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I. INTRODUCTION 


A tactical missile normally requires midcourse guidance to ensure 
that its trajectory leads to a specific target. A typical midcourse 
guidance law pre-programmed strategy maintains constant altitude, heading 
and speed. Such guidance is primarily effected by an Intertial Navi- 
gation System (INS). In this work two steady-state Kalman Filters (SKF), 
used as estimators of the longitudinal and lateral motion, constitute 
what may be considered as part of a Strapdown INS onboard a missile that 
can be cheaper and easier to implement than a gimballed INS. The authors 
of [Ref. 1] discuss the basic differences between Strapdown and gimbal led 
Inertial Navigation Systems. 

Sensors on the missile that the longitudinal and lateral estimators 
could use are described by Maybeck [Ref. 2] and include laser rate gyros, . 
doppler velocimeters, magnetic compasses, and barometric altimeters. A 
radar seeker could provide a distance or range measurement or range rate. 
Distance or position measurement could be computed from a signal inserted 
into the missile's INS from the Global Positioning System (GPS) or 
Similar satellite-based navigation system. 

This work was motivated by Bryson [Ref. 3], where he discusses a 
Strapdown INS using SKF as estimators applied to the model for the DC-8 
airplane. To avoid classification requirements and for convenience, the 
mode! used here is essentially the same as that of [Ref. 3] rather than 


that of a missile. 
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This thesis is a continuation of the work done by Matallana [Ref. 4]. 
It further investigates the sensitivity of the Kalman Filter to inaccu- 
racies in the filter parameters or varation between the filter model and 
the plant model for longitudinal motion estimation. The differences 
could be due to model inaccuracies or to normal variation caused by a 
changing flight environment. The sensitivity of rms estimate errors to 
inaccuracies or differences in the stability derivatives is the result of 
interest. 

The initial work conducted was to reproduce the results of [Ref. 3] 
and [Ref. 4] with the correct implementation of the dynamics in the 
filter parameters. Then the results of [Ref. 4] for the longitudinal 
motion estimator with incorrect implementation of the dynamics in the 
Kalman Filter were reproduced. 

After a distance measurement and associated system and measurement 
noise parameters were added to the model dynamics, the sensitivity anal- 
ysis was repeated for the longitudinal motion estimator. The analysis of 
the effect that this distance input had on the sensitivity of the rms 
errors to inaccuracies or differences in the stability derivatives of the 


Kalman Filter concluded the research for this thesis. 


Jide 





II. MODELS AND ESTMATION 


A. KALMAN FILTER 

Only a brief description of the Kalman Filter has been included to 
show the particular formulation used. A more complete development of 
general theory is done by Gelb in [Ref. 5]. 

1. Linear Dynamic System 

Consider the linear time invariant system (plant and measurement 

models) given by equation (1) below, where x represents the states of the 
system; z is the measurement; F is the system matrix; [T is the driving 
noise coefficient matrix; H is the measurement scaling matrix; and w and 
v are independent, zero-mean, white gaussian noise processes with covar- 


lance matrices Q and R respectively. 


Fx + Fw (I-a) 


«Ke 
i 


z=>Hx +v Cl=b)) 


Mathematically, Q and R are represented by equation (2) as: 


il 
© 


E(w(t)w!(t)) = Q(t)a(t-t), E(w(t)) (2-a) 


il 
© 


E(v(t)v'(t)) = R(t)a(t-t), E(v(t)) (2-b) 


2. Continuous Kalman Filter 
A continuous time Kalman Filter is described by equation (3) 


where X is the state estimate and K is a matrix of constant filter gains. 


x = FR + K(z-H%) (3) 
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The implementation of the System Model and the Kalman Filter is shown in 


Figure |. 


MATHEMATICAL MODEL 


KALMAN FILTER 





eS Be Se ee ea a a Se SS ea oS SE EE ee, ES Se se a ed 


Figure 1. System Model and Kalman Filter 


The estimate error is defined by equation (4) as 
xO8n- x (4) 


and the differential equation for X is given by 


~ 
x 


= (F-KH)x - Tw + Kv (5) 


The differential equations for the states of a linear system driven by 


noise can be expressed as 


ils 








} eee] Bb) . 


The covariance of the estimate-error, symbolized as P, is defined by 


equation (7). It provides a statistical measure of the uncertainty in x. 
p = E(x!) (7) 


The diagonal elements of the covariance matrix are the root mean square 
errors of the state variables. Also, the trace of P is the mean square 
length of the vector X. The off diagonal terms of P indicate the degree 
of cross-correlation between the elements of xX. The covariance matrix P 


is obtained by solving the linear Lyapunov equation given by 


Pp = (F-KH) P + P(F-KH)! + ror! + KRK! (8) 
The eigenvalues of the filter are given by the roots of 
ISI - F + KH] = 0 (3) 


B. STATE AUGMENTATION AND SHAPING FILTERS 

When the system random disturbances are correlated in time, i.e., 
colored noise, it is necessary to use their power spectral density data 
in order to develop a mathematical model that produces an output which 
duplicates the noise characteristics [Ref. 2]. Correlated random noises 
are taken to be state variables of a ficticious linear time invariant 
system (usually called a shaping filter) which is itself excited by white 


gaussian noise. Such a model is given by equation (10) below, where the 
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subscript f denotes filter, and n is a nonwhite (time-correlated) gaus- 
Sian noise. The filter output is used to drive the system depicted by 


Figure 2. 


Xe = F EX ¢ 1 oe (10-a) 


Zz. = HEX ¢ (10-b) 


The dimension of the state vector (1) is increased by including the 
disturbances as well as a description of the system dynamics behavior in 
appropriate rows of an enlarged F matrix. This enlargement process is 


called state vector augmentation. 





SHAPING FILTER SYSTEM 


Lp ee mata et | a= as SP SD OS ES Bw EE DH LSS pos 


3 





e@penp @ a» ap & ep oe = ap pe oe 











| ee ee ep ep ep 622 6 & ean © ep 


Figure 2. Shaping Filter Generating Driving Noise 


The augmented state equation is given by 


CHAPEL " 
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The associated measurement equation is 


= X 
«> [ells] 02) 


C. SENSITIVITY TO PARAMETER VARIATION 

Observing the structure of the Kalman Filter illustrated in Figure 1, 
the filter contains an exact model of the system dynamics. 

The analysis of how the error covariance behaves when the gain matrix 
is computed using perturbed values of the F matrix, such as varying 
parameters due to different flight conditions, is well explained in 
[Ref. 5]. Figure 3 is a block diagram of the system model] and Kalman 
Filter with the system dynamics perturbed. F* is the perturbed system 
dynamics, while K* is the associated gain matrix computed for the Kalman 


Filter. 


SYSTEM KALMAN FILTER 
c 


Cae ey ee eRe Ep Sl Rae eee Ee 





Figure 3. System Model and Kalman Filter with 
Perturbed Dynamics 
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The equation for the estimate is given by 


X = F*X + K*(2-HX) (13) 


The error in the estimate is given by 


X = (FX = K*H)X + AFX - Tw + KXv (14) 


where 


ar S Fe - F (15) 


The differential equations for the states of linear system driven by 


white gaussian noise now become 
= |F* - K*H! | ar] | x] + | K*v-rw 
| Es 4 H en .” 


N 
Letting x' be the augmented state vector, x' : A , 





The covariance matrix of x' is given by 


E(x'x!!) = [ 





t 
V 
4 i) 


where one defines P = E(xx'), V = E(xx!), and U : E(xx!). P, the covar- 
jiance of x, is the quantity of interest. The error sensitivity equations 


are: 


T 


B= (FX - KH) p + P(EX - K*H)! + af + Vag + ror! + K*RK*! (18-a) 


T 


v= FV + vcF* - K*H)! + Usk! - ror! (18-b) 


Py 





and T 


ee) eal pel (18-c) 


With initial conditions P(0) = -V(0) = U(O) = E(x(0)x(0)'). When the 


actual system dynamics are reproduced in the filter, F = F* and AF = 0, 
and equation (18) reduces to the linear Lyapunov equation of equation 


(8). 


D. MODAL COORDINATES TRANSFORMATION 

The system represented by equation (1) 1s not unique. Consider an 
alternate linear transformaton of the states described in references [3] 
and [6]. Let x = T, where € represents the transformation of the states 
and T 1s the transformation matrix with the columns formed by the eigen- 
vectors of the system matrix F (for a complex eigenvalue, the first 
column is the real part and the second is the imaginary part of the 


eigenvector). The similarity transformation of equation (1) is 


E 


7b 


AE + Bw (19-a) 


CE + y (19-b) 


=I ] 


where A=T FT, B=T I, and C =HT. 

A case of particular interest, the canonical form, results when the A 
matrix is diagonal (i.e., when the eigenvalues of the F matrix appear on 
the diagonal). This canonical form is more informative than the transfer 


function method, since observability and controlability of the system can 


be obtained by inspection. 
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E. SOLUTION OF THE SKF WITH A PRESCRIBED DEGREE OF STABILITY 

The constant gain Kalman Filter (SKF) used as an observor will 
diverge if undisturbed, neutrally stable (UNS) modes are in the system 
model. In references [3] and [7] the authors discussed the destabili- 
zation of the system model (1). The amount of destabilization can be 
varied until the suboptimal observor formed has a desired degree of 
stability. The method of [Ref. 3] destabilizes only the UNS modes in the 
system model and is called "modal destabilization" (MDS). In this tech- 


nique the gains of the filter are constrained so that 
Re(S. ) Orem Sees 2 od aoe doe yn (20) 


where Re(S, ) indicates the "real" part of (S.), Syoe-e 5S) are the eigen- 
values of the filter, i.e., the roots of equation (9), and o is a speci- 
fied positive number. 

The original system model is destabilized in accordance with equation 
(21), where fe is the destabilized matrix formed, E is the destabili- 
zation matrix (diagonal), and T is the modal transformation matrix 
(eigenvector matrix). The matrix F' is used to calculate the suboptimal 


gains of the filter. 


] 


Fi =F + TET. eA 


This MOS approach prevents the divergence of the steady-state Kalman 
Filter in a system with UNS model while causing only a slight reduction 


in the estimation accuracy. 
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III. DYNAMIC AND MEASUREMENT SYSTEM MODELS 


A. REFERENCE AXIS SYSTEM 
The Reference Axis System of a missile is centered at its center of 


gravity (c.g.) and fixed on the missile body as follows: 


X axis, the roll axis, forward from the c.g. along the axis of 
symmetry. 


Y axis, the pitch axis, outward to the right from the c.g. when 
viewing the missile from behind. 


Z axis, the yaw axis, downward from the c.g. in the plane of sym- 


metry to form a right-handed orthogonal system with the 
other two. 


Appendix A lists the symbols defining quantities associated with the 
missile illustrated in Figure 4 below such as forces and moments, linear 


and angular velocities, and moments of inertia. 





Relative Wind 


Figure 4. Reference Axis System 
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B. MISSILE EQUATIONS OF MOTION 

The equations of motion used to represent the missile dynamics used 
in this study are well defined in [Ref. 8]. A linear dynamical mode] of 
the missile based on the rigid body approximation is appropriate. 

1. Longitudinal Motion 

The longitudinal motions of a missile can be modeled by a fifth- 

order system of equation (22), where the state variables are u, velocity 
along the X axis, w, velocity along the Z axis, q, pitch rate, 8, pitch 
angle and h, altitude. The units are: u and w in 10 ft/s, q in 0.01 
rad/s, 8 in 0.01 rad, and h in 100 ft. 


Ce 


Xu Xw 0 =G 0 U 
W Zu Zw V 0 0 W 
q | = |[Mu+MwZu Mw+MwZw  Mq+MwV 0 0 q (22) 
3 0 0 1 0 0 : 
h 0 -0.1 0 V 0 h 


2. Lateral Motion 
The lateral motions of a missile are modeled by the fifth-order 
system given by equation (23), where the state variables are: 8, sideslip 
angle, r, yaw rate, p, roll rate, 6, roll angle, and w, heading angle. 


The units are: 8B in rad, r in rad/s, p in rad/s, 8 in rad, and & in rad. 


Za 





8 Yv - 0 g/V 0 8 


r Ne N21 Nr 0 0 r 
= 1 i 1 
p Lp L LS 0 0 p (23) 
b 0 0 0 0 b 
hs 0 0 0 0 y 


C. MODEL DYNAMICS 

The aerodynamic data used in this paper appears in Appendix B. 
Except for the addition of system and measurement noise parameters for 
the distance input, the models and noise dynamics are the same as those 
of [Ref. 3]. 

1. Longitudinal Motion Estimation 


The main disturbance inputs are the two wind velocities Ug and Wo: 
Under certain flight conditions, the turbulance represented by the fluc- 


tuating parts of Ug and Wo are colored noise. They are modeled by first- 


order shaping filters with white gaussian noise inputs as shown in 


equation (10). The linear model that results is given by equation (24) 


[Ref. 4]. 
-0.4 0.413 0 
Ug 13 0 Ug H 
L re (24) 
Wa 0 -0.853 Wa 0 0.853 us 


Ze 





The numerical data for the longitudinal dimensional derivatives 


was used in equation (22). 


The resultant model is represented by equa- 


tion (25) which corresponds to the state vector augmentation of equation 


(11). Scaling is done with u, w, u_, and w 


g 


g 


insudndeseon  l0eett/sS, gq mn 


units of 0.01 rad/s, 8 in units of 0.01 rad, and h in units of 100 ft. 


u -0.015 
W -0.074 
q -0.749 
m= | 0 
h 0 
| |? 
Wg 0 
0 
0 
0 
+ 10 
0 


’ 


.413 


0.004 
-0.806 
wliOe? 

0 
=o. 1 

0 

0 


0 


0.824 
-1.344 


| 
0 
0 
0 


-0.0322 
0 
0 


0.0824 


05013 
-0.074 
=0e 749 


“0.413 


(0 Seis: 
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The measurement model shown by equation (26) assumes a rate gyro 


in order to measure Zq and a barometric altimeter to measure Zp 


Z3 





(26) 
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2. Lateral Motion Estimation 
The main disturbance input is the lateral wind v. The turbulence 
represented by the fluctuating part of v is the colored noise, which is 
also modeled as a first-order shaping filter with white gaussian noise 
input as given by equation (10). The resulting shaping filter taken from 


[Ref. 3] is given by equation (27). 


Bg = “0.8538, + 0.853u (27) 


where = vy ./V. 
Bo g 
The numerical data for the lateral dimensional derivatives was 
applied in equation (23) to obtain equation (28), which corresponds to 


the state vector augmentation of equation (11). 





-0.0868 -] 0 0.03907 0 -0.0868 B 
2.14 “0.228 -0.0204 0 0 2.14 : 
-4,4] 06354 2-15 181 0 0 -4.4] p 
; 0 0 ] 0 0 860 0) . 
0 1 0 0 0 860 Us 
0 0 0 0 Oe Develoes' |B 
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The measurement model given by equation (29) below represents the case 


where the measurement z is taken with a roll-rate gyro and the measure- 


ment z obtained from a magnetic compass. 


p 
r 
Z 0 0 ] 0 0 0 p (ae Bee Vv 
= + P (29) 
Zu, 0 0 0 0 I i () gS] ut 
us 
Pg 
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IV. ANALYSIS 


A. SIMULATION 

The Sensitivity Covariance Program developed for application in the 
work of [Ref. 4] was used to solve the error sensitivity equations of 
equation (18). The program was originally developed to handle a set of 
105 linear differential equations for the longitudinal case and 78 for 
the lateral. The program was revised to accommodate 136 linear differ- 
ential equations in the longitudinal case and 101 in the lateral, to 
allow for an additional state augmentation (i.e., the distance measure- 
ment to the longitudinal model). The outputs of these programs are the 
time matrices and rms estimate errors, the square roots of the diagonal 
elements of the P matrices. The OPTSYS program, the use of which is 
described by [Ref. 9] and amplified by [Ref. 10], was applied to calcu- 
late the Kalman Filter gains to be inserted into the Sensitivity Covar- 
jance Program to find the estimate errors for specific system parameter 
perturbations. The OPTSYS program was also used to destabilize the 
systems that contained UNS modes in an attempt to eliminate filter 
divergence. Copies of the OPTSYS and the Sensitivity Covariance programs 
follow under COMPUTER PROGRAMS, while a copy of [Ref. 10] appears in 


Appendix C. 


B. RESULTS 
As the problem is introduced, the results are presented in three 
parts: (1) to verify the findings of [Ref. 3] and [Ref. 4] with the 


correct implementation of the dynamics in the filter parameters, (2) to 
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reproduce the findings of [Ref. 4] for the longitudinal motion estimator 
with incorrect implementation of the dynamics in the Kalman Filter, and 
(3) to conduct a sensitivity analysis of the longitudinal motion esti- 
mator after adding a distance measurement and associated system and 
measurement noise parameters to the model dynamics. 
1. Motion Estimation Analysis for Exact Dynamics 

The OPTSYS program was used with input data representing the 
actual system dynamics for both the longitudinal and lateral cases to 
obtain the following results which are the same as those of [Ref. 4] and 
essentially the same as those of [Ref. 3]. 


a. Longitudinal Case 


filter gain matrix K filter eigenvalues 
0.059 0.060 =0es 10 30.411 
0.264 =Omlel -0.429 

ool 0.040 SORaZ3 

0.001 -0.080 =0.26 1 

= 014011 ] 020385 -0.063 + j0.0743 
SleZoo 0.128 


rms estimate errors 


u = 2.090 ft/s 8 = 0.317 deg 
we oe Ogmit/s h = 8.245 ft 

q = 0.416 deg/s Ug = 4e7oot/s 
Wa = ool tts 
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b. Lateral Case 


filter gain matrix K filter eigenvalues 
0.057 -0. 967 eeesou) + 722594 

= leaekohe 0.411 -0.624 + j0.492 
2.695 -0.004 SOP U0n25 

0.386 -0.789 0.0 

= (05 elas) 0.906 

Ae igs 0.655 


rms estimate errors 


¥. = 3.329 ft/s 


B 

r = 0.244 deg/s 

pp 808377 dea/s 

> = 0.222 deg 

t = 0.214 deg 
"s =750506 ft/s 


2. Longitudinal Motion Estimation Analysis 


The OPTSYS program was used to compute a new K* matrix as each 
parameter of the F matrix was individually numerically varied. The 
Sensitivity Covariance Program was then executed utilizing each new kK* 
and F* matrix pair to determine the rms errors for each individual per- 
turbation. 

The results are shown in Tables 1-8 and are identical to those of 
[Ref. 4]. The true values for the unperturbed system dynamics parameters 
are indicated in the tables by an asterisk. A discussion of the results 


follows: 
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The dimensional variation of the X force with forward speed u 
has a nominal value of -0.015. This quantity was varied ina 
range of +20%. The behavior of the rms estimate errors can be 
seen in Table 1. The tabulation shows that the numerical 
variation of the ae derivative does not cause significant 
changes in the nominal values of the rms estimate errors of the 
states W, q, 6, u , and w.. The states u and h appear to be 
slightly effected, but not enough to be of importance. 


The dimensional variation of the X force with downward speed w 
has a nominal value of 0.004. Again, a numerical variation in 
a range of +20% was conducted. The behavior of the rms errors 
1s demonstrated by Table 2. Comparing these values with the 
nominal ones reveals that changes in the XY derivative have 
essentially no effect on the states w, q, 8, u_, and w_, while 
the states u and h show changes too small to consider important. 


The dimensional variation of the Z force caused by a change in 
the forward speed u has a nominal value of -0.074. The design 
value was altered in a range of +20% with the results shown in 
Table 3. Evaluation of this data indicates that all the rms 
errors show some sensitivity except for that of q. The most 
significant changes occur in the u, 8, and h states. The large 
variation in u can be important in terms of the accuracy in 
radial position. 


The dimensional variation of the Z force with downward speed w 
has a nominal value of -0.806. The results for this case with 
changes in Li over a range of +20% follow in Table 4. They 
show that all the rms estimate errors are quite sensitive and 
any variation of Ze beyond +2% can be considered critical] and 


unacceptable. 


As, 





TABLE 1. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN x DERIVATIVE 


* | |e] dele |e |e | te 


-0. 0.018 | 2.096 096 5.102 | 102 8. | 8.248 4. 4.776 | a Pen 














TABLE 2. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN Ky DERIVATIVE 


Mie. || aio |4 | %| % 


iD). } 0.0048 Ce, | 2.070 3. | 5.102 O. 0.416 ) 0.317 | ot7 Be | 8.319 | 4. 4.776 | 5): | 5.701 
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TABLE 3. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN Ly DERIVATIVE 


Me l&lu|e/ hie ele 


0.0888 0888 | 1.885 885 | 5.106 106 0. 0.416 | 0. 0.322 | | 9.310 310 4. 4.776 2). | 5.707 | 














TABLE 4. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN ZL. DERIVATIVE 


ae eee 


(07 -0.9612 Si): 30.08 6. 6.172 0. 0.486 0.440 440 j 17. 17.97 | 4. 4.778 ie | 7.138 















cue 





lc 


le 


lo* 


<= 


this work. 


Table 9. 


The dimensional variation of the M moment caused by a change in 
the forward speed u has a nominal value of -0.000786. From 
Table 5, one notes that the rms errors for the states q, u : 
and Wa are not effected by a variation in MY of +20%, but 
significant changes are seen when MY is varied more than +10% 
in the errors of states u, w, 6, and h. 


The dimensional variation of the M moment with speed w has a 
nominal value of -0.0111. The results of a numerical variation 
in a range of +10% can be seen in Table 6. Since any alter- 
ation in the true value of MY has a strong effect on all the 
rms estimate errors, this derivative can be considered the most 
critical in the longitudinal motion estimation case. 


The dimensional variation of the pitching moment with pitch 
rate q has a nominal value of -0.924. The results of Table 7 
on the rms estimate errors for a +20% change in Mg show that 
the sensitivity to variations in this parameter is minimal for 
all states. 


The dimensional variation of the pitching moment with the rate 
of change of the downward speed w has a nominal value of 
-0.00051. Table 8 contains the rms errors data obtained by 
altering Me +20% from its nominal value. All the errors show a 
degree of sensitivity and the variation of errors is signifi- 


cant when Me is changed by more than +2%. 


Since plots of the rms estimate errors versus changes in the 
particular dimensional derivatives for data identical to that of Tables 


1-8 appears in [Ref. 4] in Figures 35-40, they will not be repeated in 


A summary of the relative sensitivity of the rms estimate 


errors to changes in the individual dimensional derivatives follows in 


eZ 





TABLE 5. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN My DERIVATIVE 


ot | del del de) & | A | ee] 


Bi 0.000943 | 2284 234 | 5.061 | 061 i | 6.230 4. eas | 5.689 | 689 











TABLE 6. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
~ WITH VARIATION IN Ly DERIVATIVE 


SS 


0.01165 01165 18.43 | 43 | 8.350 | 350 | 0.502 502 0. | 0.455 | asl 29.870 4. | 4.778 oe ) 5.695 
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TABLE 7. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN Ma DERIVATIVE 


|6—C J BRE 


=" 1.109 | | 2.091 09] 5.102 102 0.416 416 Oe 0.316 | 8. | 8.230 4. 4.776 °. 5.701, 





















TABLE 8. RMS ESTIMATE ERRORS FOR LONGITUDINAL MOTION ESTIMATOR 
WITH VARIATION IN ne DERIVATIVE 


Se 


BU. 0.00067 | Ze | 2.610 | | 5.053 053 Q. | 0.417 0.273 ZH 10.665. | 665 4. 4.775 ) 5.688 688 
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TABLE 9. RELATIVE SENSITIVITY OF THE RMS ESTIMATE ERRORS 
TO CHANGES IN DERIVATIVES 


DERIVATIVE 


Cc = os = Cc 


= 


oO 


X 
X 
ib 
U 
M 
M 
M 
M 


=e 





NS = Not sensitive 
RS = Relatively sensitive 
VS = Very sensitive 
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augmentation of the system and measurement models 


3. Analysis of Longitudinal Motion Estimation After Augmentation 


Including the distance measurement to the system required state 


equations (30) and (31) that follow: 


CDe Oe 


Je 


Cre 


Qa a 


0-015 
-0.074 
“0.749 


ll 
© 


0 
0 
0 


.413 





Oe 


004 


= UL cll 


= 10: 


0 


aa 0. 


i 


] 


858 


0 


0.824 
-1.344 
ll. 


a> SS cs © 


0 


-0.0322 0 
0 0 
0 0 
0 0 
0.824 0 
0 0 
0 0 
0 0 

Hy 

Hy 

Eq 
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-0.074 
“0.749 


as demonstrated by 


0.004 of |u 
- 0.806 Q W 
caill) Ud) 0 q 
0 ol je 
0 Oo] Th 
0 0 
~G 
-0.853 0 
"g 
0 Oo} Id 
(30) 





and 


Z rol 0 0 0 0 «(OO 1 O 0 V 

q q 
z,| = 5) 10) 10 SC (0! 0a 7 41 eles 0 Vi ly 
zy oO 0 0 0 0 |] Oe | Va 





In these equations the state variables are u, velocity along the X axis, 
w, velocity along the Z axis, q, pitch rate, 8, pitch angle, h, altitude 
and d, distance traveled along the X axis. The units are: u and w in 
10 ft/s, q in 0.0] rad/s, 6 in 0.01 rad, h in 100 ft, and d in 10 ft. 
The OPTSYS program, when executed with the data from equations 
(30) and (31) above and the standard deviation values from Appendix B, 
yielded the following: 
filter gain matrix K 

0 Woks 0.0570 0.0014 

0.2646 = Oracle -0.0007 

3.5168 0.0404 0.0002 

0.0011 -0.0810 =O O07 

0.0135 Ca ese 0.0001 

-0.0114 0.0356 -0. 0002 

=li2576 0.1283 0.0005 

0.0469 0.0561 1.0014 


oy 





filter eigenvalues 
=saros + 4, 1103 


-1.000 

-0.4146 

=0MS 152 

-0.0485 + 0.0548 
0.0551 


rms estimate errors 


y= 2.080) Ree @ = 0.316 deg 
6 = 5 a2 foe h = 8.225 ft 
q = 0.416 deg/s Ug = 4.776 ft/s 
Wq = 5.10) ee a) SS 7 O sae 


The next step in the analysis was to perturb each directional 
derivative ndenendént ly by specific amounts from its nominal value and 
to observe the effect on the rms estimate errors. This process was 
carried out for all eight directional derivatives and the response was 
the same for each case -- even a slight perturbation of -0.1% of any 
directional derivative from its nominal value caused the rms estimate 
errors to increase without bound. This behavior indicated that any 
incorrect implementation of dynamics in the new system formed by the 
augmentation of the distance eeaeacenent would cause instability and the 
Kalman Filter to diverge. 

Several system parameters were individually modified and the 


analysis repeated in hopes of finding a stable system for which the 
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Kalman Filter converged. The coefficient for the distance term in the 
process noise distribution matrix was varied from 0.01-5.0, the power 
spectral density process noise entry for distance was changed in a range 
of 1.105-30.0, and the distance term for the power spectral density 
measurement noise adjusted over a range of 0.03-30.0. None of these 
trials led to a stable system. 

A modal analysis was also performed using the open loop eigen- 
values from the OPTSYS output listing. The system of equations (30) and 


(31) when transformed into modal coordinates give equations (32) and (33) 


be low: 
Ee 00 0 0 0 0 0 0 Ee 
EY 00 0 0 0 0 0 0 E4 
Ea 0 0 -1.076 2.957 0 0 0 0 Eo 
ope |0 0 2.957 -1.076 0 0 0 0 ea 
s 0-0 O 0 -0.006 0.024 0 0 E51 
52 00 0 0 0.024 -0.006 0 0 E52 
Eu i 00 0 0 0 -0.413 0 Eu, 
EM 00 0 0 0 0 0 -0. 853 EW 
0.1 0 
0 0 1.0 
.028 -0.088 0 
Hy 
.110 -3.153 0 
+ Ly (32) 
.027 -0.048 0 
ae 


-210 1.467 0 
-420 0 0 





lest * 1! 
eis) 





Z 


Z 


Sq 
, 0 0 0 0 -0.0001 0.0005 0.0548 #0.4802 E65] 
.| = 1.0 0 0.0016 0.0016 -0.0156 -0.0612 0.0082 -0.0025| + E69 
4 fee ).0 -0.0008 -0.0004 1.0 0 =), 065795 -070252 Sp) 
52 
Eu 
Ew 
] 0 600 
“g 
+10 1 Q Vi (33) 
0 QO ] Va 


Consistent with the discussion in [Ref. 3], inspection of equa- 
tion (32) revealed that the energy mode Ee and the distance mode Ea were 
neutrally stable (1.e., eigenvalues = 0). Inspection of equation (33) 
showed that ge was unobservable with Za and Z4 and that a was unobserv- 
able with Zq and Z,. Equation (32) also disclosed that Er was undis- 
turbed by Ug and d, and that Ed was undisturbed by Wq° Therefore, de- 
Stabilization was conducted in an attempt to prevent filter divergence. 
Both total and modal destabilization described earlier in this work and 
in [Ref. 3] were performed in amounts of 0.040 and 1.0 using the OPTSYS 
program. The filter gains computed for the destabilized system were then 
executed in the Sensitivity Covariance Program with each of the modified 
Darameter combinations discussed earlier. Without exception, the rms 
estimation errors increased without bound when the least sensitive dimen- 


Sional derivative xy was perturbed by as little as +1%. 
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V. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 
The conclusions reached were based on the results obtained and will 
therefore be presented in three parts. 
]. Motion Estimation Analysis for Exact Dynamics 
The data in the results is in agreement with that of both 
[Ref. 3] and [Ref. 4]. It shows that both the Kalman Filters for initial 
longitudinal and lateral cases are stable when the true values for the 
system dynamics are implemented. 
2. Longitudinal Motion Estimation Analysis 
The results from Tables 1-8 and summarized in Table 9 are con- 
sistent with those of [Ref. 4]. The stability derivatives Z and Mo 
cause the strongest changes in the rms estimate errors when they are 
varied. Basically, d. and M must be quite accurately reproduced in the 
filter to prevent divergence. Changes in the stability derivatives Ly 
Mp and Me reflect intermediate variations in nearly all the rms estimate 
errors. For the model considered a tolerance of more than +5% affects 
the accuracy in the radial position since large variations in u occur. A 
tollerance of perhaps +20% can be accepted in the dimensional derivatives 
X Xp and Ma for this model] since no important effect is noted in the 


rms errors over that range. 


3. Analysis of Longitudinal Motion Estimation After Augmentation 


From the results presented earlier for the new system model fomed 


by the augmentation of a distance measurement and associated process and 
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measurement noise parameters, it is apparent that the corresponding 
Kalman Filter will diverge for even a slight variation in any of the 
dimensional derivatives from their nominal values. Even the Kalman 
Filter developed by system destabilization proved to be unstable with the 


parameters used. 


B. RECOMMENDATIONS 

Further analysis of the augmented system including the distance or 
position estimation is desirable. Perhaps a more in-depth study of the 
Measurement parameter scaling would enable the development of a stable 


Kalman Filter for at least a destabilized system. 
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VI. SUMMARY 


The sensitivity analyses performed in this work have revealed the 
importance of accuracy in determining system dynamics utilized in formu- 
lating the model for the Kalman Filter. The relative sensitivity of the 
rms estimation errors to variance in each of the particular dimensional 
derivatives is shown in Table 9 for the Longitudinal Motion Estimator. 

The longitudinal system augmented with the distance measurement 
developed appears to be extremely sensitive to variations in all the 
dimensional derivatives. Further analysis of the model developed is 


suggested. 
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Regular Symbols 
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APPENDIX A 
ErSieUE SYMBOLS 


Definition 
Modal transformation of F matrix 
Modal transformation of FT matrix 
Modal transformation of H matrix 
Dutch roll mode 
Distance traveled along the X axis 
Destabilization matrix 
System dynamics matrix 
Subscript for filter 
Destabilized matrix 
Subscript for wind speed 
Measurement matrix 
Heading Mode 
Altitude 
Inertial Navigation System 
Kalman Filter gain matrix 
Rolling moment (about X axis) 
Pitching moment (about Y axis) 
Modal destabilization 
Yawing moment (about Z axis) 
Non-white gaussian noise 


Covariance propagation of the estimate 
error matrix 


Perturbed rol! rate 
Covariance matrix of w 
Perturbed pitch rate 
Covariance matrix of v 
Perturbed yaw rate 


Spiral mode 
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Regular Symbols 
SKF 


7 
UNS 


O 


NN ~< Ke KOM KK EF EF < < Cc 


Greek Symbols 
Ws 


a ure Q Q 17RD & @D 


Definition 
Steady-State Kalman Filter 
Transformation matrix 
Undisturbed neutrally stable 
Perturbed forward speed (along X axis) 
Forward velocity 
Perturbed side velocity 
Driving white gaussian noise 
Perturbed downward velocity 
Reference axis 
State vector of the system 
State estimate vector 
Estimate error vector 
Reference axis 
Measurement vector 
Reference Axis 


Definition 
Heading angle 
Perturbed pitch attitude angle 
Perturbed bank (roll) angle 
Sideslip angle 
Driving noise matrix 
Eigenvalue constrain 
Standard deviation 
Transformed state vector 


Time 
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I. 


Longitudinal Model 


a. 


APPENDIX B 


AERODYNAMIC DATA AND PROBABILISTIC INFORMATION 


v = 820 ft/s 


Dimenstonal Derivatives 


Xu = 0.015 
Xw = 0.004 
Zu = -0.074 
Zw = -0.0806 
Mu = -0.0786 
Mw = -0.0111 
Mq = -0.924 
Mw = -0.00051 


l/s 

l/s 

l/s 

l/s 
Isat 
Wig aaete 
1/s-rad 
litae 


Distrubance Noise Standard Deviation 


Q 
iN 
Q 
I 


Q 
I 


1.105 1/s (10 ft/s)2 
30.0 1/s (10 ft/s)? 


(7 ft/s rms gust with a 930-ft correlation distance). 


Observation Noise Standard Deviation 


Q 
i 


0.15 s (0.01 rad/s)? 
0.05 s (100 ft)? 
30.0 s (10 ft)? 
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2. Lateral Models 
a. Dimensional Derivatives 
Y,, = 70.0868 1/s 
No = 2.14 I/s 
ee eUaceo 17s 
Nn = -0.0204 1/s 
L, = -4.41~  1/s? 
L. = 0.334 I/s 
LS = -1.181 1/s 


b. Disturbance Noise Standard Deviation 
eras ieccex 10h ys 


(7 ft/s rms gust with a 930-ft correlation distance) 


{ 
c. Observation Noise Standard Deviation 


o =1.5x10° s 


p 
Oy, ° ess 


1.5 x 10° 
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APPENDIX C 
AN AID TO USING OPTSYS AT NPS 


I. INTRODUCTION 


One of the tasks involved in my thesis work at the Naval Postgraduate 
School (NPS) was to verify some of the data of reference [1] which in- 
vestigated the sensitivity of the Steady-State Kalman Filters as lateral 
and longitudinal estimators in Strapdown Inertial Navigation Systems 
(INS). One of the recurring, essential calculations was for the steady- 
state gains of each system model considered. Fortunately, the OPTSYS 
computer program was available in Fortran at the computer center to help 
perform this enormous job. The use of the OPTSYS program was covered by 
reference [2], but not in adequate detail for easy application. After 
much trial-and-error, frustration, attempted decoding with the assistance 
of the computer center staff, and prayer, and at the expense of many 
man-hours of time, our Lord enabled me to properly fill out and order the 
data cards for a particular modeled system and obtain the expected 
results upon execution of the program. Since Professor Collins has 
several otner students in need of a users working knowledge of the OPISYS 
program and anyone using Kalman Filters can benefit as well, I am writing 
a more detailed description of how to correctly input data by discussing 
a specific example. The intent of this paper is to supplement the 


guidance of reference [2] and further facilitate research at NPS. 
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IT. MODEL AND ESTIMATION 


Consider the linear time-invariant system given by 


Fx + Tw 


xe 
tl 


Hx + Vv 


N 
il 


where x represents the states of the system; z is the measurement vector; 
F is the system matrix; T is the driving noise coefficient matrix; H is 
the measurement scaling matrix; and w and v are independent, zero-mean, 
white gaussian noise processes with covariance matrices Q and R, 
respectively. 


A continuous time Kalman Filter for this system is described by 


a 


X= FX + K(z - HX) 


where x is the state estimate and K is the matrix of the steady-state 
gains of the Kalman Filter. The implementation of the System Model and 


the Kalman Filter are shown below in Figure C-1 [Ref. 1]. 
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2 eee eee 


MATHEMATICAL MODEL 


KALMAN FILTER 


MEASUREMENT 


SYSTEM 





Fr = eee wb 8. 8 SS SS SS SS Oe SS es a ee Sa oe ae SE SS Sa ee a ee | 


System Model and Kalman Filter 


C= ile 


Figure 
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AN EXAMPLE OF LONGITUDINAL MOTION ESTIMATION 


After state vector augmentation, the resultant model of longitudinal 


motion of an aircraft of the form x 


uU -0.015 
W -0.074 
q -0.749 
aaie= | 0 
h 0 
B |’ 
oe 0 
0 
0 
0 
+] 0 
0 
0.413 
0 


0.004 
= oOG 
SO. 7 


0.853 


0 


0.824 
~1.344 


] 
0 


Fx + Tw is 


» 0822 


. 0824 


0 


SOs OS 
-0.074 
-Q. 749 
0 
0 
-0.413 


0.004 
=OF 306 
7 10n7 


S288 


= 


om 


=> 


Cc 
Ce 


= 
Ce 


where the units are scaled such that u, w, u_, and Wa must be multiplied 


g 


by 10 to give feet per second, q by 0.01 to give radians per second, 8 by 


0.01 to give radians, and h by 100 to give units of feet [Ref. 1]. 
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The corresponding measurement model in the form z = Hx + Iv is given 


by 


N 

© 
© 
i 
© 
© 
© 
© 


q (26) 


© 
© 
© 
© 
oa 
© 
© 


ys 
ii 
x Cc => @ .O = Cc 
QQ Wa 
+ 
= 


For this model Qu = Qy = 05m 10 Tt/s)+/s.; Ro = 0.15 (0.01 rad/s)? and 


mee) 0.05 (100 ft)- s (Ref. 3]. 
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IV. APPLYING OPTSYS TO THE EXAMPLE 


The essential input data that will enable OPTSYS to calculate the 
Steady-state gains of the Kalman Filter and many other parameters out- 
lined in [Ref. 2] follows on page 55. The input data and control cards 
are described in the paragraphs below. 

Card | - The 17 entries in every other column from column 2 through 
column 34 essentially tell OPTSYS what to compute. See [Ref. 2] for more 
details. 

Card 2 - The 5 entries in every third column from 3 through 15 de- 
Scribe the system being modeled to OPTSYS. The first entry tells the 
number of states or order of the system-/7 since there are seven rows in 
the F matrix. The second entry gives the number of controls-0O since u=0. 
The third entry tells that we have 2 measurements, while the fourth entry 
Shows that two process noise sources exist. The fifth entry is always 
zero when filter synthesis is done. See [Ref. 2] if regulator synthesis 
only is desired. 

Cards 3-16 - These cards contain the F matrix. The first six entries 
of each row go on one card with 12 columns for each entry-1-12, 13-24, 

..., 60-72. The seventh entry for each row is placed in columns 1-12 of 
a continuation card that immediately follows the card with the first six 
entries of the row. Note that if our example system were 6x6, the F 


matrix would only take up cards 3-8. 


oe 





The next three cards, 17-19 in our example, contain the H matrix. 
Note that this matrix is also entered on the cards by rows, but consecu- 
tively with an entry in every 12 columns with 6 entries per card as long 
as unused row elements remain! Thus the first entry of row 2 of the H 
matrix appears in columns 13-24 of card 18. 

The next three cards, 20-22, hold the T matrix. This matrix is also 
entered consecutively by rows with an entry in the first 14 groups of 12 
columns on the cards! 

The next to the last card gives the Q matrix. Note that this card 
has only the diagonal terms of the matrix in columns 1-12 and 13-24. See 
{Ref. 2] for matrices with non-diagonal terms. 

The last card is for the R matrix and also has diagonal entries in 
columns 1-12 and 13-24. Again refer to [Ref. 2] if non-diagonal terms 
exist. 

This supplement will be effective until the OPTSYS program is 
re-coded in WATFIV language. Its usage should greatly improve the 
efficienty and morale of those using the OPTSYS program on file at NPS 


Computer Center. 
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IP(INQ «20. 1} Note D maou 
WRITE (6, 6012 

WRITE (6,6000}) (AY(I),I=1,NO) 
9045 WRITE (6.6001 
606 WRITE Pueaent Sirsa) ed = 1,Nq 

R = 

IP(782NE. 1) L (edt gdh § 

CALL MODE ( NORNT, G,BM,NS,NS,NC, 0) 
8501 CONTINUE 

IF(IOL . FQ. 3) GO 70 8505 

WRITE (6, 6003) 

DO 607 L=1,NC 
6 WRITE (6, 6000) (B(I4d) 351 NC) 
8399 IF(ITP1 .£Q. 0) Go TO’ 8406 


WRITE (6, 9220) 

FORMAT (60°, 77,2X, "OPEN LOOP TRANSFER PUNCTIONS...') 

CALL TF NS ,NS,NSQ,BA, AA, No, G, BM, NO,HI»CH,IFDFN,D, BB, (ooo CP, 
ra I Cag cal, SC, 5 abs R&S,D1,D2,DDD, BPS, ITF1,1TF 

8400 IF mail JNE. 8 


02 
IF (NG Eh 0 ) aerua 
GO TO 62 
&502 CONTINUE 

nO TR 320: 


eres 250. 3} oe 10 9130 
cerareen CeeeeRronnGcnscbosGutes> coo es | 
C*@®SCALCULATION OP CONTROL GAINS: FORMATION OP CONTROL HAMILTONIAN 


07 

e 99 
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Cc 
Cc ff $ OS8P AND FT ARE THE OPEN LONP 
ce 2 : DYNAMICS 4ATRIX AND TRANSPOSE 
c Of P -GN¢ BISG MT? OFSBI IS NCXNC CONTROL WEIGHTING 
Cian? = MATRIX 
Cc. 6|O ® eeOk IS THE NSXNS STATE wWEIGHTIN 
coc : MATRIX 
c of ~A -PT & 
Cc ar a> O8ESH IS THE NSXNC CONTROL 
Cc DISTRIBUTION MATRIX 
Che TOarer ge rgnrr HF SZSHSOSCSSERANSUGOGARRECGANOES SAP RIrKrunsonazsaeeycisreklsas 
DO 24 I = 1,NC 
DO 34 J = 1°48 
2u PRO (I, J) = G (J, AT) /B EL, 1) 
= e 
nO 52 3 = 1.8H 
RM(I,J+H) =. D0 
po 28 Kk = 1,KC 
25 RA(I J+ nH = EM(1,J+M $ (TK) 2 PRO(K J) 
Ct?" 2NX2N AAMILTONIAS SO RPRLX SreRNesscacene ss 
C---DIAGONAL BLOCK3---%11 AND M22 
DO 26 I = 1,4H 
DO 26 J = 1.4H 
RM(I,J) = Pa (Led) 
PM¢I¢MH,J¢MH) = -BA(J,Z) 
GaeeMi21 BLOCK 
26 FM(I¢HH,J) =-RM(T¢MH J) 
f=--MdOmSLOCK ISe DEFINED. IN LIVE 25 ABOVE 
CORMaCBSOBCCREONLASSSSBS*OSIBSrKIAASENBDATH.EINS 
50 CONTINUE 
feerpeeiGc .20. 0) GO TO 1950 
WRITE 6,60 14) 
6014 POPFAT(//,* EULEP-LAGRANGE SYSTEM MATRIX...',//) 
Greer Aetna M9 eahs4 8 (9( 1%, 1PD13.6)) *) 
1050 CALL EALANC(4,8, 7% Low LAT aH. DI 
CALL OKTHES (4,8,LOU,7 HIGH, 2 ®, D2 
CALL ORTEAN(S,%,LO@,IHIGH,RM, D2 x) 
CALL HOR2 (RS row, rhrcu, £4, wh wf, ,LERR) 
Tepiske swe. Sy cALL EPEXIE (4 ¢R A, TEER) 
CALL SALFAK (4,2 ,LOW, THIGH, D1, S, a} 
Ctescersarerecuncocensécseafrcasctantanketess 
@ DEBUG DIAGNOSTICS OW £-L EO 


60 





IF(IDEBUG .EQ. 0) GO TO 53 
WR TE (59115) 
9115 PORHAT(7/7/" ZIGVAL AND EIGNVEC OF 2°N E-L EQ. APTER HQR2'//) 
D =1,5 
52 WRI TE (69116) WR(I) pwI(I) 
9116 FORHAT (TE, 12 D13.6) 
WRITE (9417) 
9117 FORMAT ( eh 
CALL FAPRNT (MoM .M,9,X,4,'(9 (1X, 1PD13.6)) *) 
S53 CONTINUE 
IF(IDSTAB .£Q. 1) GO TO 54 
IF (NCB.FO.0) WRITE 3119) 
IF {NOB.NE.O} WEITE (6.9121 
S54 IP (NOB.NE. 0) GO TO 60 
9119 FORSAT('O',//,2X, *EIGENSYSE EM OF OPTIMAL CLOSED LOOF SYSTEM..',// 
9121 FORMAT (+ 08 577 2X; (EIGENSYST ES OP ESTIMATE FEROR EQUATION.....°, 
ALL nGAT y M,NS,NC,NOB, WR, WI,X,GN,W11,RM, ) 
1W21,D1,CWKR,C#I, SC, MHS ,D2)_ 
C CHECK ESGVEC 
IF(IDEBUG .£0. 7 GO TO 759 
WR TE (6, 9125) 
9125 FORMAT(! FIGENVECTORS FROM RGAIN PRIOR TO CNORM') 
CALL SAERNT (NS,NS,NS,9,SC,3%,° (9(1X, 1PD13.6)) °) 
750 CONTINUE 


¢ Pesott uA wennO  F MATRIX FOR ITERATIVE DESTABILIZATION CASE 


IF (JDSTAB_- EQ. 0) GO TO 9136 
9135 I=1,NS 
9135 ah (I, ,it) = BACI,1I) - DESTAB(I) 


9136 CONTINUE 
C CALCULATION OF FEEDBACK GAIN 


c 
CY®S PFEFDBACK GAINS---0 = ~(BINVERSE)2GTIGN 
Peete oe. 


§01 I = 1,NC 
BPO tT J} = 0° D6 
&4 6 He as 


aH 

O PROT £9), wae oe J) yok. eT) PEN {Kd a 
1 FBS x Jp PRO (I 

- IF(IDSTAB -2Q. 1) ae TO 9130 

C NORMALIZ 
Cc 


80 
80 


= AND PRINT OPT. REG. CLOSED LOOP EIGENSYSTEM 
IWRITE =2 
{CALL CHORM (CHR, CWI,SC,NS,IWRITZ,NSQ,DDD, D1, D2, WNORM, WNOPBI,FRGC, 
AA, NC, NS) 
COONTHE OPTIMUM PEEDBACK CONTROL GAINS 
9139 WRT Ze (6,977) 
977 FORMAT(///,' ‘,"THE CONTROL GAINS AEE:',/S/) 
DO 966 I = 1,N 
968 WRITE (6, 978) (AGC {T, J Bon od a 
078 POREA { © 2X, 12860 iy Sok abis. 6) 
Cc CoseEuTe wodaL € sfreix 
C OPEN LOUP O-INVE SAVED IN WNORSTI 
IF {If NE. 1) GO TO 985 
C IN COMPUTING MODAL C RECOMPUTE U OPEN LOOP 
C SINCE wWhORH USED TO STOR U AND U-INSV PIR CLOSED LIOP SYSTEMS, AND 
C WNORMI USED TO SAV2 U-INV OP2N LOOP 
DO 8510 I=1,NS 
DO 6510 J=1,NS 
8510 WNOEH (Todt = WHORSI a) 
CALL ATSY NS @NORM Lae DD, D1,D2) 
CALL SODE Sms ha PBGE, AL, NS, NC.NS,3) 
985 CONTINUE 
CteFTHE CLOSED LOOP DYNAMICS SATBIX 
DO 160 I = 1,NS 
90 160 J = 1,N3 
SUS = 0.0 
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IWRITE=5 
CALL orRsh Pe GN NS, 8°. 05,NSO, DDD, 01,02, 8sORM  @sOrSI,HO,AA, 
KS 


“po 9416 I=1, 
$e Esk 3 ay ae 20) GOTO 9810 
6 4 
FORERGZ//7 4. PHOGRAM TERMI KATING DUE TO UNSTABLE PILTER") 


CONTINUE 
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9780 IF {¥C. EQ. 0) GO TO 202 
no 190 fr = 1,NS 
DO 190 3 = 1,NC 
PRO i J). = 0.D0 

191 DRO a1, 2 phorr J Teen) Eos C td. K} 
: J} = PF Do eGmiy ; ae : 

190 CONTI NDE uae) ( ( 
DO 200 fT = 1,NHNC 
DO 200 J = 1,KC 
SC(I,J) = 0-50 

201 ae 251 : Sc (Loa J) +P BGC (I, K) ? PRO(K,J) 

~ = ’ +: ’ e 

200 CONSENUE 

202 IF(IREG -EQ. 0) GO TO 9791 
DO 9792 I=1,NS 

DO 9792 J=1)NS 
corr, J)=Ga (f£,4 
C020 243 (hes) 

9791 WEEE (6 ( 220) 

220 FORMA { Of 7/,2X,'THE COVARIANC® OF THE ESTIMATE..',//) 
CALL RAPRNT (M4, M&,MH,5,GM,4,' (5 (1X, 12D13.6)) *) 
IF(IzR.GT.2) Goto §03 
DO 67 I = 1,MH 

67 CO ey Z hth J) +GM(I,J) 
+8 = e es ¢ 

503 ee RuE 
WRITE (6 210) 

FORMAT(' 0", 77,2X ‘THE STATE COVARIANCZ MATRIX..',//) 
CALL RAPRNT (MG, SE.MH,5,CQ,4,* (5 (1X, 1PD13.6)) *) 
IP (8C2EQ. 0 cé to" 343° 
WRITE (6 21 
221 FORMAT('O",//,1X,'THE CONTROL COVARTANCE',//) 
DO 230 I = 1,NC 

230 WRITE (6,231) (SCeL, Jd= 1, NC) 

231 FORMAT(1P6D14.6) 

232 DO 230 2 = 1,NS 

240 cgi I) = DSORT (C CO (T I)) 

I f&C- EQ. 0} GO 35 1 

250 Sc ey Z Sica I)) 

= WY 

351 WAL TEE, 262} 

262 FORMAT('O', 1X,*STATE RMS RESPONSE',201, "CONTROL RMS RESPONSE!, 
aA 

0 270 I1=1,NS 
IF (teLe nc) CRITE sg y222h S SQ (lel Se (lat) 
aro SGnnAgT(* *,1°D15.7, 
Ip (I.Gz.NC) WRITE (6, See Che, I) 
270 CONTINUE 
: 389 IP(ITF3 .2Q. 0) GO TO 440d 
C PORM COSPENSATOR FROM MEAS TO INPUT AND COSPUTE TP 
DO 410 I=1,NS 
DO 410 aa 
SU4=0.D0 
DO 405 K=1, 

¥0S SUM=SUMm+¢PaG ie K) @HO(K, J) 

a10 CO(I,J) =ACL (1,9) 
aR TE (6 924 

9240 roReAT(fO", ie 2X,'CCMPENSATOR TRANSFER FUNCTIONS...*) 
TP Xe 
IZZP9=0 
CALL TP(NS,NS,3SO,CQ,AA, NO, FBGE, Be, AGE e= 712-809, 88. 6C,C 2, 
r) pete icaP Cut soc, JC?,RES, D1, 02,000,EPS,ITP3, ITFK} 
uuQ CONTINGE ~ 
C COMEUTE PSD FUNCTIONS OF THE CONTHROLL2D SYSTE 
LE (TPSD -2Q. 0) GO TO 450 
CALL PSD Sees ac Su .GV, F832, 80, HY, #0, HO, PEGE, NG 
GP "3 ae 
1 GALE, ACL,B ce ae pi, 62,5 eo oc, ac. 36,ce,1 ,£PSD, Lxokn) 
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NANAN 


O 


- 


CALL PSDCAL (4,NS,RM,X,NC GH ,GV, PBGC, NO, HY, HY, 40, PEGE, NG 
aGhtg ACL pBA,a RHE, DI, §2, SCP RES, Q,RC, BB,Ce,2 ,£PSD, LNOKM) 
Y4u4 CALL PSDCAL(S,NS,8M,X,NC,G®,GV, FBGC,NO, HY, HU, 40, FAGE, NG 
1 Gin, ACL BAL GR, aE, OL, Doce RES, 0,2C, B8,ccC,169,1 PSD, 1NORM} 
uS0 IF( iss 50. 0) RETURN 
IF(NC .NE. 0) GO TO 395 
DO 390 I=1,NS 
390 RCL iy l3) =" 82 (I 3) 
fo e 
395 Consr hue 
CALL MINT(NSQ,ACL,NS, DDD,D1,D2) 
READ (5, 7444) (a8 (7s i= ani} 
WRITE (6¢ 9771) eR (2) f=1,43) 
9771 FORMAT(*O',' STEADY SIsTORBANCE VECTOR. 2-322" yf 10 (1X, 1PD16.6/)) 
9765 FORMAT /GLes,' STEADY STATE VALUES JF STATE VAR. ARE.~-..-... 
WRITE , 976 , 
DO 9763 I=1,NS 
WI(I) =0.0 
DO 9763 J=1,NG 
9763 W(I) aT (I) #GAM (I,J) 6 #R (J) 
DO 9764 IT=1,NS 
CR(I} 20.0 
9768 CRIT Se RII) {ACL (I J)? ®I (J) 
+ a aR a 
9764 Eat TE Is, 000) crf) 
DO 49766 I=1,NC 
CI (I) =0.0 
DO 9766 J=1,NS 
9766 Gasth (6. 43b3) “cee I J) SCR (3) 
WRIT 46 767) rf) 5) 
9767 FORMAT(///,'* S gtare EQNTROLELS foes cas o'eS//e1O(IPDIS. SJ) 
RETURN 
END 
SUBROUTINE CDIV (A,B,C,D,&, F) 
IMPLICIT REALS (A-H, 0- ee 
THIS SUBROUTINE COMPUTES THE COMPLEX DIVISION 
E+ FOI = (A * BREID/(C + DeT) 
T=COC+DE D 
E=(AIC+ETD 
F=\pocearD /T 
RETURN 
END 
SUBROUTINE FAPRNT(NMAX,N,N, L,A, IDIM, FST) 
REAL@A A(NEAX MN) 
DIMENSION PST (IDIM) 
NU=L 
NO 20 NL=1,N,L 
IF (NU. GT. fy NU=N 
90 10 I=1,4 
10 WRITE 6 88a) (ACI J) ,»J=NL,NU) 
RITE (6, 100 
100 FoRSAT(® ') 
20 NUFKUCL 
RETURN 
END 
SUBROUTINE RGAIN(M, NS /SC,NOB, RWI, VP, GN, zw 11,768. 
WOT, LTPC,CI Co, ANS y AT) 
reeLrcf{t"REALe6 ca-#, 0-2) 
DIMENSION apie) et (ef VE (, ee NS, 5), 
DESeNSTON Wiicss, NS a (3 1(NS,NS) ,LTONSD, ST (NS) 
DISENSION SRS CUUNS, SCT 
z 
KP = 1 
KN = 1 
NBZEV = 0 
NCPZEV = 
NOmtPEX.Gtos) GO TO 200 
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HECK POR EIGVAL AT OR NEAR J-DM2GA AXIS TO INCLUDE IN E-L FIGSYS 
TORN PIRST ONE POSITIVE AND SECOND ONE YEGATIVE 
ELGVR=DAES (WR (K ib 
THPEECVR oC. 10) GO rd 48 
Te }RT (X)) 40 40 
30 NRZEV = NRZEV 
EP (NRZEY -GT. 1) GO TO 35 
BR(K) = EIGVR 
GO TO 75 
35 RR (K) = -EIGVR 
RIT 46, 3000) 
9000 FORHAT(‘O%,' EULER-LAGRANSE EQUATIONS HAVE A REAL EIGENVALUE AT*, 
' On NEAR ZERO.'/) 
5 TO 110 
uO NCPZEV = NCPZEV41 
IF(NCPZEV .GT. 1) GO TO 45 
ERK) = EIGVR 
WR Ke 1) = EIGYR 
GO To 8O 
uS WE(K) =-EIGVR 
WR K+1) = -EIGVE 
WE TE (6, 9010) 
9010 FORMA { O'." EULER-LAGRANSE FQUATIONS HAVE A COMPLEX PAIR OF ', 
© ‘EIGENVALUES AT OR NEAR THE J-OMESA AXIS.°) 
GO TO 120 
UB IF(WR(K)) 100,50,50 
50 IP eT (RL{ 80 80, 75,80 
w- ern - =e GENVECTOR FOR REAL SIGENVALIE, POSITIVE 
75 IF(NOB. EQ. 0) GO TO 78 
DO 76 J = 1,N 
76 TCB (J KP) ='VF(J,K) 
78 KP = KPe 
K=K 41 
GO TO 10 
~---------FIGENVECTOR POR COMPLEX EIGENVALUE, POSITIVE FEAL PART 
80 IF(NOB .FQ. 0) GO TO 83 
DO 81 J = 1,3 
FR = VF (J,K 
FI = -VF(3,K#1) 
TC | KP) = FR¢PI 
81 TCB{I,KP41) = PR-PI 
83 KP = fp+2 


100 TF (4 (KY) 120,1 120 
Bee eS ee Be FIGENVECTOR FOR REAL EIGENVALUE, NEGATIVE REAL PART 
110 c KN) = wR (K) 
Clik } = q (K) 
IF (NOB.NE. 0) GO TO 96 
KNS = KNeNS 
DO 95 J= 1,8 
95 TCB(J,KNS) = VP (J,K) 
96 KX = Ne] 
=K¢1 
GO TO 10 
~--------- EIGENVECTOR POR COMPLEX EIGENVALUE, NEGATIVE REAL PART 
120 &2 = Hee 
hI = WItK 
C(KN) = & 
C{R Ne a a 
e cee) = RY 
CI{FN¢1) = - R 
IP (NOB.NE. 0) GO TO 122 
KS = ¥NeNS 
20 121 3 = ee 
FR = ¥ 
Pr = vee Ske 
[CB (J,KNS) = FR¢PI 
121 TCB(J.khS*1) = PR-PY 
122 KS = kye2 
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COLUENS 
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at 


ky Sao sc > 
INTERCAHANG 
L245 (K) 


a5 





ANAN 


naan 


ANN 


AanNA ANN 


ANN 


38 


40 


45 
46 


48 
50 
55 


~~ 


ees 


AD MRUUse wri 
OM “=~ il UwKOT I 


A2rP"r OF Fr} 
i} 


me ©r0,0,0 Gir 


AINED IN BIGA) 
) 48,46,48 

1,N 

56,55,50 

A (IK) /(-BIGA) 


ASsHHO WOH 


DUCE MATRIX 


A> BHHHOHW THO 
HOLM OHHTHI Se DD UN RHI! oO} 
mr CP PC RU op OR RI OH 


Zit lis il 
a) 

Cc, OO 

ae SS) 


+A (IJ) 


DIVIDE ROW BY PIVOT 
KJ=K-N 
DO 75 J=1,N 
KJ=KI4«N 
iy uiesish 70,.75,70 

A (KI) /B EGA 

LFODUCT OP PIVOTS 
D=Df BIGA 

PFPLACE PIVOT BY RECIPROCAL 


KK} = (1.0D0) /BIGA 


80 CONT 


100 
105 
108 


ss a od 
RO fh = 
ww Oo 


FINAL ROW AND COLUMN INT ERCHANGE 


=o 


) 
150, 150,105 


) 120,120,108 


-1 
ona 


ae 


a; \ (3) 
LD 


100,100,125 
I21,8 


it Ce ee 
| ame | 


th omy, te Oy Ce 1 EH 
i oR CO dle a) SS a 


OMMmC, wr OO Cy hHtH 
Ong ti VR HOMOwWO WY I tl 
Wwiimna 1d 901 x- 1 


O.4 a} 
os 
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NANANANANAAAQAANNA 


DRANDDHDASAH OH 
MOUNT 


AO 


40 ey Plame ty = X(IT J} 
= e us (i, ¢ 
DO 42 T=t NL 
RN) cae 
x =0. 
DO 41° 3J=1,NR 
U4 X © J)aX (Led) +Q (1,53) THR (I, J) 
42 CONTINUE 
RETURN 
END 
SUBROUTINE MODE (WNORM,G,GNO RM,NS,N1,N2,ICCOy) 
WNORM TRANSPORMATION MATRIX U OR U-INV 
NS NO. OF STATE 
NC NO. OF INPOTS OR OUTPUTS 
ICON CONTROL PLAG TO INDICATE WHICH TRANSPOS “ATION 
0 = MODAL G 
1 = MODAL GAMMA 
2 = MODAL H 
3 = “MODAL C 
u = MODAL K 
S = CONTROL FIGENVECTOR MATRIX 
6 = MEASUREMENT EIGENVECTOR MATAIX 
IMPLICIT REALS (A-H,0-Z) 
DIMENSION WNORM NS oil) G (Nt N2 eGNORM (M1 “2 
00 FORMAT('O', //,2%,*HODAL CONTROL DISTRIBUSIOY MATRIX..'°,//) 
O1 FORHAT{'0",//,2X,"SODAL PRICESS NOISE DISTEI=UTION SATRIX..', //) 
70 PORMAT(*O' ;7/,2X,"MODAL MEASUREMENT SCALING FATRIX'. //} ‘ 
HOG EORANG(///,° °, "HE SODAL CONTROL GAINS APE:* // 4 
84 FORMAT('O',/,* CONTROL EISENVECTOR 4ATRIX_-¢ 77 
86 FORMAT('O',/,' MBASUREMENT EISENVECTOR SATRIK.',//) 
00 PORMAT(' *, (2x IPEDS .6)) 
90 FORMAT('O', "NODAL PILTER STEADY STATE GAINS......°,//) 
DO 50 l=1, "1 
DO 50 J=1,N2 
50 GNO RM (T J) =0. 
IPOINT={£CON¢1 
GO TO (75,75, 250, 250, 75, 250, 250) , IPOINT 
75 DO 100 J=t,N2 
DO 100 I=1,NS 
00 CN RH I J) EGiORA I,J) *WNORS (I,K) °G(K, J) 
oN 4 = : v + 4 g 
Garo. (1c 130° 5405960, 109) thot ut 
05 WRITE (6 6500) 
10 DO {=1 
20 BRITE (6, 6000) (GNORM (Z, J) ,J2 1,52) 
U 


KET 
150 WRT TE (6 6501) 
GO 140 
160 Gar TE (¢ 6590) 
GO To 140 
250 DO 504 J=1,NS 


DO 260 I=1,N1 
po 260 Ks \, NS 
260 GNOKA (Ly = GNORM (Ty Sf J owNOPs(K,.) 
ne: a, DG 261), 2 y 261° 630h 64) ,IP tht 
261 ter E (8 48540 
262 soe eee 
GO TO 270 


263 WRITE (6 6584) 

GO TO 0 

dBrTE 10 (£586 
=1 


Ze E (6. *6060) (GNORR (2 Pals eile iS) 
RETUR 


END 
SUBROUTINE CNORM(WZ,#Y,VEL, NS,TWPIT2,95Q,DDD. 1.02, WNORM. WNOE 8 
HO,CM,N1,82) , 7 2D1,02,8NORE, WNOK SI, 


w2(I) PEAL PART OP I-TH BIGZNVALUP 


/2 





aAaANANNARANANNANANANANN 


10.0 0'010:1010'0'0\016 
a st 4 OOD0O000 


Oo =&- WNHBOUODWANEw 


AAAANN 
i 


990 
ze) Tl 


980 


991 
el ele, 


OoOO009000000 
_ 


a¥(I) COMPLEX PART OF I-TH EIGENVALUE 
VPC MATRIX OF RISHT ZIGENVECTORS STORED IN REAL FORM 
FROM HOR2 

NS NO. OP STATES 

IWRITE FLAG TO CONTROL PORMATS FOR DIFFERENT FIGHENSYSTEMS 

WNORM NORMALIZED MATRIX U OF RIGAT EIGENVECTORS STORED 

BY COLUMNS IN &EAL FORS 
WNORMI O-INVERSE 2*CCNGUGATE JP LEPT PIGENVECTORS 
TORED BY 23OW IK REAL PORS 

NSQ,DDD,D1, D3 - ARGUMENTS PASS2D TO MINV 
IMPLICIT REALSB AnH, 0-2) 
REALS 8 FIELD, COMMA, S=MCOL, RIGHT, FMT 

IMENSION RZ(NS) Lae (HS), WES (NS Hoy, ETO Er a 

gHNORNT | S,NS) , STORE (6) ,DICNS),D2(NS), FST (1%) ,HO(N1,N2), 
Wee H(NIEN2) SPGONUN7Z OM 82, 71,5 ENCOL/SH, ':*, 7 

gRIGHT/ 1H) / FMT/OH (1X,1P,43£1H 7 SEMEND/MH, 2° 7 
FORMAT('O', "OPEN LOOP EIGENVALUES... ] 
PORMAT('O'. "CLOSED LOOP OPIIMAL REGULATOR EIGENVALUES..') 
FORMAT(*°O' .*CLOSED LOOP SUBOPTIMAL REGULATOR SIGENVALUES..') 
FORMAT(*'O',*CLOSED LOOP OPTIMAL ESTIMATOR ZIGENVALUES..*) 
FORMAT('O'.*CLOSED LOOP SUBOPTIMAL FSTIMATOR EISENVALUBS..") 
FORMAT(/s*O', "RIGHT EIGENVECTOR MATRIX. °//) 
FORMAT(//'O', "OPEN LOOP LEPT EIGENVEDTOR TAZRIX. « 1) 
FORMAT(//'0', "CLOSED LOOP DPT. REG. LEPT EIGENVECTOR MATPIX.. '‘) 
FORMAT(//*0', "CLOSED LOOP SUBOPT. RES. LEFT EIGENVECTOR MATRIX..° 
FCREAT(//'0', ‘CLOSED LOOP DPT. FILTER LEFT EIGENVECTOR MATRIX. .1*) 
FORMAT(//'0", "CLOSED LOOP SUBOPT. PILITER LEFT EIGENVECTOR MATRIX. 


FORMAT (96X,° (',F10.7,°) #3 (*,F10.7, "1 °} 


HOM iepeweemr LEX ZSIGENVECTORS BY LARGEST ELEMENT 


KK=9 
LR=9 

Lc=) 

ro 399 K =1,NS 

TE (KK EQ. 1) GOTO 991 

IP (DAES (WY (K))~LT.1.D-10) GO ro 999 
LC=LC +1 

=MAX = 0.DO 

DO 997 I = 1,NS 

CMOD=VEC (I,K Neegerrc (2 Ke1)o@2 
iF (CHODIEAAX 997,990,990 

SMAX = COD 

“= 

CONTINOE 

V4R = VEC(S,K) 

WMI = VEC(#,K¢1) 

99 980 r=t, 4s 

VR = VEC (I,K) 

VI «= VEC Ty ket) 

VICSNE(VR® VNR Lo yrry EMAX 
VFSIN={-YROVNLe VIVE (FEEKK 
ZeORMS 7K) SVECRN 

WKORM (1. F4#1) =VECIN 

CosTrRNus 

KF=1 

S0TO 999 

KE20 

CONTINUE 


Cc 
CeO PENI Ce ee eAL ELGENVECTIORS SY THE TOTAL LENGTH 
Cc 


90 1000 F=1 
oP (DABS (a¥ (K) ) - GE- 1.D-10) 3sOTO 1009 
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ANN 


998 


996 


ow 


520 
530 
540 
545 


550 
56 0 


2 It 
9 FBAWNAOVUOOCr 
O HAHaENOltowoonm 
il — 


pae2+RE BOD 
D) 


mem WAI WO 
= p41 td 1m, et 0 


/RSOD 
EG 


AANxnwowmwowe 
O02 <O*rOms 
zzOwm Or 

LZImAWIING + 


WUWOm Goer wna 
Oo- WN mer OHO 
Own 


GO 530,590 7585,550) ,IWRITE 
WRITE (0, 90 30) 


GO TO 
WRITE (6, 9040) 
GO TO 56 

AT celenc 20) 


GRITE (6, 9060) 
to S86 


GO 

a aa 9070) 

KK=0 

NPaTw=0 

NFA TW=1 

ee 568 I=, 1 


TO $6 
TFs DABS EQ; 1), Go eGie -10) KK=1 


PRINT OUT NC “MORE THAN 6 WORDS, NOT SEPARATING COMPLEX EIGVAL 


561 


562 


Win 
na 
@~ 


IF(NPHTW LLT. 5 ,OP. (NPRTW -EQ. 5 .AND. KK .FQ. 0)) GO TO 561 
FM eee = RIGHT 
WRITE (9, PST (STORE (J) ,J=1, NPRTW) 


oe TO 562 
ELD 


mCOL 


oP 


Hi 
wgtiowc i 


“aan We 
—witkt Ore) 


Oe. wa MLMOwom” 


me ONO Urr © ejetiqg DH 
Qo 
Ve) 
On 


OUNS Om won Od a, 


@RIT? (6.9110) 
mOr6 3 
WEITE (6,9120) 
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fe am" 
K'O GIO 


100 


8000 PORMA it 


9000 


OOP IN WNORS 
Bj GO 10 1095 


ox 

H 2 
tS) . 

oo | 


Oo 
a be (1) p> Toe oO Or 


[ oll eal I ood el ©) 
ph amt od HT} 


“he (1x, Ted 13-6) ) ") 


 ANORRE, Ks 


NS, NS, 6 


NORMS 
NS, Néve 


ve) 


D2 
2) 
,'(6(1X,1PD13.6)) °*) 


be oe SO ool Be 
2° x 


mt 2 Ceres sn 


% 
H4icwoOowmeimO Ww 
ZV1-yumOMNN<C SMEs RHOOW <a. 


Cy 
Qe Ce Gat HOO KK CULAR HHH 


es Ans BN,L,C,CN,IfFDFry 
PU oe, DDD, EPS, etx) 


$,D 
N, BON als etl) Chet LL, f D Pay 
yEvt (ined RM) APL (8 fe? Ly oScé ky, 


CL SYS WITH MODAL WORK DONE IN CPTSYS 


eH 


On 
He 
ad <2 (iY 
J 
| 
ae Tl ee 
Zz 


om ~~. 
AFPOOR OM 


He 2Ore H 


“ze 8 
tre OO Ornor is =x 


yore 1) 


Geoger Tote 2 Cr. sc} 
rbues 


buior 
ss Punm-sy:= 


om 


C(I,J) 


2 * 


oe 
=a, 
i 
= 
ZS 


* C(I,K)*AA(K,J 
) wobo 61,02) 0 


2a ilewoWlworty? § eos ms 


m Wigdem Gr Cres 


29 


C, 
EB2V* CMAN BZOASMPNAZZD —OOre NYS wer 


I,J) + AA (I,K) * B(K, J) 


- a 


=2 Cy 


PILE OE WONEN™, AAA, 4, 3,L,C,D.S6;CC,CP, EVR, SVI,D1, D2, 
g2) CALL RESIO(I ,J,N,JC?,4,B8,L,C4,PR,PI,RES,BB,CC,1} 


9% 
NGCMYMANH NAHOONWOUDONNONVOSFOCOVUNNANG 


QOaTz VOW KSB VYONODINADABRIO KW ROH OOFrOOC# Nh) 
rie =! GH 


cS Mer 


fay) ~-} 
“0° (92 toN 7H 


N Woda uw SO ee 


HZ Ta ee a UO me ee rt © 


ees ,ITFt, ,ITP2,ITF3,1FDFW,1IE,IL0STAB 
3 ENS R JL TF REQUESTED 

Peo a 

T 

P 2) -CO™rs 25 

MUST BE INPUT, I.E. NOB .ST. O°) 


- 


ec 
Seo 


m™ ON 


s,IM 
I for 
¥s 0 
LOL 
@ 


rr roszNnc za NyN HOO 


O* OID 


OQ re 
MOO ”W @—KHOHNM 


+410 OH 


iret O 
e 


FrITOOORFrPHOrO MH1p FOOrer GC OO + & OG OO 
wn 


I 


ee 
tFf 
LE 
Ck 

Hf 


om ON 
“Ne 
Sanat 


STOP 
25 CONTINUE 


TRANSPER FUNCTION CHECKS 
I2 .EQ. 0) LE=6 
EPs =10.3 a8 (TE) 
OPEN LOC 
EP(ter) 38 EQ. 0 -OR. NC .SE . 0) GO TD 50 
er rs 
SPOR EAI (7 7" °t sous Sy MATEIX MUST EE REQUESTED(I-2. NC NE. 0) °, 
70 COMPUT? OPES LOOP T. F. °) 


ee 





2) 
a 4 


WO 
oo 


ve] 
=e 
(o) 
(o) 


0) GO TO 100 

OPeaNDs (NG UNE. O .AND. NG .NE. OF) GO TO 100 
Ee 

E 


GULLS Onan LLTER SYNIHESTIS SUST SE REQUESTED IN ° 
RUN TO COMPUTE COMPENSATOR T. F.') 


= Bri ws 


HO 


% “ 0 
QeHHMIAMN MmrHHON 


o} 
be 
Oo 


3 a 0) GO TO 1590 
E a: NC .NE. 9) GO TO 150 


) 
BOSE cee oe CaLe ULATSD ONLY WHEN REGULATOR DESIGNED * 
AMMA INPUT, I. &. NG .NE. O°) 


Omrs+y OFF Ot rg h 
Dht~aaH ZO Whe..wZO0 


WO 
N 
Oo 
© 
G 


° #tNyOr H 


AM 

oh 

Wal os | 

Ot” 

_ & tn 
OW OHWInmQn SHAME Nee FH OR ft ER} OW 
ae 

“NK 

It) 2 2 


9300 


rNes 


* FN WoW on 


tht else 6 
20 


PTION DESIGNED FOR A REGULATOR OR ', 
MULT ANEQUSLY *) 


MePmanrvere 
td‘ ri. 


GS Cen me *& HH) 


200 
Cc 
= °2SD 
Cc 


om 
ra 


2 OFS Ogi» 
ty 


3 


ao eGO., LO 250 
Ze GO, TO, 250 
GTI. NG¢N3) GO TO 250 


Zo0 
9400 


Za 
9500 


ere ENO vorS LENT CESD INPUT FLAGS serena” ' } 
8 5 O .AND. NC .NE. 0) GO TO 300 
(95 


Sees ODOT AeAeheot LALOR AND FILTZP MUST BE PESIDENT °*, 
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Tera 63§O 


9) GO TO 780 
} Z DCMPLX (8,9) 


2” Or 


ro 


eee by =e W(t) = 6 I(T) = 

250. 0.000) VR = PAGHE 
BS(X) ¢* DABS(Y \ - PaaS | 
»X®S-ZZASA-QTE A) OC™P 
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p * NORM 
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GHmipcew av cara <2: 22223 2% 


BACK SUBSTITUTION. 
Ono OF et SOLAeD nhOOLS 22 cst ccs: ¢ 


OW .~.AND. I .~L2. IGH) 


Ste CeNO SAHIN WG HK 


08 tO fe, Ls Oe Thee a) Dr] 4 


ee yee 
te Oe 
oe ete 
ee pjee 
es pdee 
ee 


*- ina 


ot 
oO 
d=] 
© 
@ 
& 
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820 
840 CONT a 
THIX. 
0 


eeeree8 @ @ #€ & @ 


DO 8980 ras = 


DO 880 I = Low, IGH 
z ODO 
K = LOW, 4 
Z¢ Z2(I,k) * 4(K,J) 
= 22 


D 
860 Zz 


aa 
880 CONTI NU 


eo ee eee ese 8&8 @ 
ee *eeees eee 


anon =o 
aA 


CON TO AN 
DUE AFTER 30 WS 


sear ew ee Orewa Owe ewe ew ee ew ew ew Ow eB Se KN SB eww we Oe STZ ww eZT FFT wwe ZZ FZ FZ FZ FZ FZ Ze FFF @e 83 Be ws ow 


TINE SALBAK (NS, N,LOW, IGH, SCALE, &, Z) 


NII, N#, 13H, LO@ 
ELL ehh 2 KE. A) 


~£Q. 0) GO TO 200 
GH .E9Q. LOW) GO TO 12) 
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ANN 


aN 


AQ ANN AANA 


AN 


100 
110 


120 


130 
140 
200 


40 


50 


60 


DO 110 I = LOW, IGH 
Ss = Sees es 
See e sceeree oe ian HAND EIGENVECTORS ARE SACK TEANSFORMED 
IF THE FOREGOINS STATEMENT IS REPLACED BY 
Se oDa/ SCALE (EL) . 323533523323 
DO 100 J = 1, 4 
(I,J) = Z(I,J) * S 
CONTINUE 
see testes FOR I=LOW—1 STEP -1 UNTIL 1, 
PGa* Wester  WoONT EG N DO == s2ssr23222 
pO 180 Iy = 1, N 
f= oF 
[Fhe eGe.e LOW SAND. I .LE. IGH}) SO TO 180 
IF {5 - LT. LOW) IX = LOW - II 
K = mage ttl 
IF (Kk 2 20. )} GO TO 140 
pO 130 J = 1, M 
Ss = get 
Z(I,J) = (K, J) 
Z(K,J) = S 
CONTINGE 
CONTINUE 
RETURN 
eee = = DASE CARD OF SAL BAK ::222::2:322 
END 


SUBROUTINE HQR(NM,N,LOW,IGH,H,WR,WI,J ESR) 


INTEGER I,J,X,L,%,N Be ee eet EON AN 2, ENE 2, LERR 


HEM ONT, GR Nf Nj 
ek: bene y YY o2) NORM, MACHEP 


re 
g kz, DABS, bsi ch 


fe ele elec) 


TED BY BALANC 

Rew ORE 222233233 3 
040 J = K, N 

CR& = NOSM + DABS (H(I,J)) 


. PANDemlee Gee ©GH) so TO 50 


ee SEARCH FOR NEXT ELGENVALUES t:t2ttti2:: 
aLT. LOW GO TO 1091 


Sosa che ORT On Ss lia Le SMALL SUS-TCIAGONAL ELZASNT 
noe oN ae ee =) UNL IL LOd PO ——= stst2tt:ss: 

ib ercouw, En 

FN * LO@ = LL 

ye 2f0. LOa) GO TO 130 
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NN 


nM 


C 
100 


S = DABS (H(L-1 We. + DASS(H(L,L)) 
te {Ba 2508 9.950) 5 Seen pers 
( (L e e M rah ) GO Eas 
80 CONTINUE “~ 100 
Sores = PON SHLET 22232 222: 2 
X= H (EN, EN) 
TE dL aeo: a GO TO 270 
Y = H(NA,NA 
w= HEN NA H (MA, EN) 
IF (L .£O. ay @ 0 
Ir (Its JJEC. GO TO 1000 
IF (ITS .NE. 10! ~AND. ITS . NE. 20) GO tO +2 
Seer epee a ORY o| EXCEPTIONAL SHipe iets cs: so 3s ck 
T=T + X -* 
DO 120 I = LOW, EN 
120 H(I,1) = H(I,I) - Xx 
S = DABS (H(EN,NA)) + DAS8S(H (NA, ENM2)) 
x = 0.75p0 2 § 
= X 
W = -0.4375D0 ® $s # 5§ 
130 ITS = ITS + 1 
ce eee LOOK, FOR 180 CONSECUTIVE SMALL 
SUB-DIAGONAL ELEMENTS. 
FOR M=EN-2 STEP -1 UNTIL L DO ~........... 
DOMOMEM= G0 PRNO))  ° 288888535 
“ = ENM2 ¢ L- 
ZZ = H(M,S) 
R=X- 22 
: i As W) / H(M41 H 
= S - Ww) N11, 8) + H(4,Me? 
Q = A(tede gen) a2 = Roe: Ss 
= H(M+2, +1 
S = DABS (P) ¢ DABS (Q) * DABS (R) 
P =P / 
OQOmoO / S 
R=z=R/S/S 
IF Ena Body) 2 (DABS { )_* DA 
ein + BS (Ry Os 
x ¢ (DABS (HlM<1-4-1)) * DABSicz) © D ) .LE. MACHEP © DABS(P 
140 CONTINUE | , y i 6 qt (ase mae GO 70 {E} 
150 MP2 = 4 + 2 
. DO 160 I = MP2, EN 
H(TeL-2) = §.0D0 
Se cl <5. nP2) GC TO 150 
H(t I-3) = 0.0bD0 
160 conTINGE POUBLE QR STEP INVOLVING 
55556 565 48 U Q ST NVOL N ROWS 
COLUMNS 4 TO EN cs:s::2222: 1. TO EN AND 
DO 260 K = S, NA 
NOTLAS = K .NB. NA 
1? (Ge 13 4) GO TO 170 
D> = eee 
g = H(K+1,K-1) 
= 0.0D0 
IP (NOTLAS) R = H(K*#2,K-1) 
X_= DABS (?) ¢ DABS (9) ¢ DABS (R) 
IP (Xx are 9.0D0) nO 260 
Px Pp / 
Q7rO7 &X 
R=kSYX 
170 S = DSIGN(DSQRT roves 
Tek £0. “) S ¢ 8) 
ad oe) “tbo 
120 IF (L .85&. M) H(K,R-1) = -H(K,K-1) 
190 P=spes 
y=, 7 S 
Y¥o=mO 7 S 
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ANANAANANANANN 


Z2.= 85/7 -S 
Q=Q/ P 
a ny 
2stsics353 ROW MODIFICATION 22222:35535 
DO 210 J = K, EN 
P = H(K,Jf + Q & H (K+ Vod) 
IF (.8OT. NOTLAS) GO TO 200 
Pee ee et J) 
H(K* 24) = H (Rt Zed - D® 22 
200 H(K+1,J3) = H(K*#1,J) - P& Y 
IK J) = H(K,J) - s X 
210 CONTINOE 
J = Be NRT | 
Seeeiseeeccese ses Luni menGUDTFLCAT ION 2222 222° 23 
DO 230 T= L, J 
D=X 3 HI Bt + Y & ee SeM. 
IF (.NOT. NOTLAS) co T 
P= P «#¢ Zo © R(I,K 
H(I,K#2) = # TKS) - 2 & 
220 H(I,Ke1) = H(I,*+1) - > : Q 
ol SA os eS 
230 CONTINGE 
260 CONTINUE 
GO TO 70 
eee > pone ROOT FOUND = ¢:¢2 o322: 
270 WR(EN) = X + T 
WI(EN) = 0.0DO 
EN = NA 
GO TO 60 
Sass wee O ROOTS FOUND <:ts222:222 
280 P = so =<) ff Ze 
oS €e p+ WW 
oZ = DSQRT (DABS (Q))} 
Xm X + T 
RreitOeslr. O. Saha cc TO 32) 
wes esccess REAL PAT cosas os 5 2 $8 
ZZ = P ¢ DSIGA eae toy 
WR(NA) = X + Z 
WRUENY = WR (NA} 
IP ee 7NE. OFUD0O) WR{EN) = X- WY ZZ 
wre a = 0.0D0 
WIC EN} = 0.0D0 
GO TO 330 
eee es ee TOCONPLEX. PAIN S22 2233223 
320 Tales) 2 x > 6 
E! = + 
WIQGNA) = ZZ 
WICEN) = -22 
330 EN = ENM2 
GO TO 60 
s22 223222: SET ERROR -- NO CONVERGENZDTS TO AN 
ELGlMVALUC AP ION SO IFTESATIONS *222¢222223 
1007 BESURN © 8 
rae N 
tee LAS GARD OF HJR 2:3 3223322 
ED 
SUBEOUTINE PSDCAL(N2 NC,Ga,37 ecaiO, HY,du,#8 
1 SSGE, NG GAM, ACL, Ghost. ne * b>, SCE, RES. 0.8, 88.00, 110, 
2 IPSD.INORM) 
PSDCAL COSNPUTES THP FSD OP OUTFUTS OR CONTSOLS OF 
A CCNTS&OLLED SYSi Ea 
Iyo= 1 OUTPUT PSD 
=z 2 CORTESOL ?SD 
IPSD=1 PSD 
=2 PSD AND TP SESIDUES 
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ANAAAN 


INORM= 1-2-ece NS NORMALIZED 3Y ITH PROCESS NOISE 
nGe4, 05. NG+#NO NORMALIZED BY ITH S2AS NOISE 
DOUBLE PRECISION FA,X,GW,3¥,C,HY,i,FBGE,GAM,ACL,2,WE,WI,D1,D2,RES 
1 B8,CC,0,R, PSD, ¥, DNORH, ONT, SMAX, SLOG, ENOD, DE, S7,OM,RE, AL, 5U, Dal 
COMPLEX 16 2D, 2N. 22 
DIMENSION FA(N2 2) X (N29N2) GH (N2,N3) C(NCoNS) gHE (NO,NZ) 
1 H(NO ,NS) PaGS(fs, B) eck NS,NG) -ACL(NS NS) ,FINS ,NS) ,@R(NO), 
Bone (x2) 1 (2) D2 (N2) (RES Not off a} & (NO ,NO) PSD (30), 
3 X Oy BB (2) Ec(n2),6V(N2, NOf, duc(Nc, N2) ,DW1(4 
INTEGER JCF 2) 
DATA DwW1/1.D0,2.D0,5. D0, 10. D0/ 
‘e 
IF(IYU .£Q. 0) IYu=1 
IF THORS 7EQ. 0) INORM = 1 
z IPF({IPSD .GT. 1) IPT = 1 
IX = INORM - NG 
IF ( IX MGT 0 WRITE (6 8000) Ix 
8000 FOPMAT(/* SUBSEQUENT PSD IS NORMALIZED BY MEAS NO.',T3) 
IP(IX .L. 0) HRITE (6 8010) INORM 
8010 FOR RAT (7 SUBSEQUENT SD IS NCRMALIZED BY PROCESS NOISE NO. ',13) 
; NSQ = N24N2 
= See ee COMP Umer EYIGENSYS TEM OFSCONTROLLED SYSTEM 
Cc cel eee ss 2 6 eS FORM @ra 
DO 10 I=1,NSs 
PA oa) a hes Te} 
10 FAIRS *e, 3) = d2 50 
90°20 I=1,NS 
DO 20 J=1,NS 
ST = 0.DO 
DO 15 K=1,NO 
18 ST = sT + FRGE(Z,K) CH (K, 9) 
FACI,NS+)) = -ST 
20 FAU(HS*¢I,NS4J) = B69) - sf 
CALL RAfENT(N2,N2,N2,9,PA,3,°(9(1X, 1PD13-6)) *) 
Ctecenn-sercnnres pEBUG ABOVE 
aArsrunAc*ITaSsCAM**ACIrcecse".2aAan 


CALL BALANC(N2,N2,PA, LOW,IHIGH, D1 

CALL ORTHPS (N2,N2,LOW,IHIGH, PA, D2 

CALL ORTRAN (N2,N2, LOW THIGH (FA, D2 Ky 

GALL HQF2(N-, N2, LOW rhicH, PA, wk, wt,X, TERR) 


Te(TER ; 0 G0 fo 1006 ae 
CA A AK (N N Ow HIGH N 
ee ee oO he a Be. Cn She hs Gaaecres 
CAINS EPNT UNG Na Nz, 9,X,4, °* (9( 1X, 1PD13.6 : 
Pom esonaceneose Nchue”’ .sbve gies Aci Re ae 
100 CONTINUE 
fs ee es DE TERNLNE SODAL MATRICES 
TREE U6 £06 ult GO TO 130 ‘ 
eG ete ss ees aU OU 
DO 110° r21,NC 
DO ee el! Nz 
ST = - DO 
DO 105 K21,NS 
NOS tSnese ct = (1 ,K)°X (K, J) 
110 HU(I,J) = Si 
GO. tO) 450 
Sl epee boil is ib 
130 DO 1890 JT=1,NO 
DO 180 J=1,N2 
57 = 0.90 
DO 135 k31,NS 
135 ST = St + H(T,K)°X(K,J) - H (L,K) SK(NS#K, 3) 
140 HY(T, 3) = s- 
CALL PAERNT (NO,NO, 42, Se ese Co ee ee 1 3.6) pp") 
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te A Ghar as tr x DEBUG ABOVE 


150 CALL MIKY 59 K,N2,ST,D1,D2) 
CALL RAPR rt BO N2 Nile Oe Xed, §(9(1%,1PD13.6)) *) 
CU SSK SIA SO SBIIOLS wenae ABOY ve 
2 322.2 See) 6 GSUbe 
DO 160 I=1,N2 
DO 160 J=1,NG 
ST = 0.000 
DO 155 K=1,NS 
155 ST = ST - kK (TN S+#K) "GAM(K,J) 
160 GRIT. J) = § 
CALL’ RAPRNT (N2,N2,NG, 9,GW,%,'(9(1X, 1PD13.6)) *) 
cttsoorrucesneyses DEBUG’ ABSVE 
cae. 2 2 eee. = US E Se Be NOR SALIZATION 
200 PE (Thoge -LE. NG) DNORM = 1.D0/0 (INORS, INORS) 
IF(INORM .GT. a) DNORM = 1: DO/K (INORR* NG INOR 
C lisiiitit: DETERMINE BANDWIDTH OF CONTROLLED §S 
EMAX = 0.D0 
DO 210 I=1,N2 
ESD = DABS (WR(T) $2 + WI (I) 72) 
TF (EXOD ~-GTe EMAX) EMAX = OD 
210 CONTINUE 
EMOD = DSQRT(EMAX) ; 
EMO D=20EMOD 
Cc 2 eee: 6 6ROUUND UP. TO NEAR EST 2,4,5,8, 10 
SLOG = DLOG10 (EMOD 
Te(EL0 ~LT. 0.00) Y20W = ee oe + 
EMAX = EMOD™1080(-rpoOwWw 
IP(EMAX .GT. 2.D0) ENOD = 2.D0 
IF(EMAX .GT. 4.DO) ZEMOD = 4&.DO 
IF(EMAX .GT. 5.DO0) EMOD = 3.D0 
IF{EMAX .GT. 8.DO) EMOD = 8.D0 
IF(EMAX .GE. 10.D ) EMOD = 10.D0 
EMAX = FMOD?10#®@IPOW 
DW = EMAX/20.D0 
Cc = = ADD 1G PONS 3s DSGADES Je 
IP(ENOD .LT. 5.0) GO TO 212 
FMAX = 1.0D1 
IK = 3 
GO TO 216 
212 EMAX = 5.D0 
IK = 2 
216 CONTINUE 
Cc eee ee SORE 30’ FREQUENCIES 
* NO 220 121,20 : 
220 W (ED = DW% (I-1) 
D 18 r=1,3 
IP = 20 ¢ 323 (I-1) 
DO 218 J=1,3 
IX = SOD (IK+#J-1,3) ¢ 1 
Jd c=20 
IF (IK FQ. 2 | SAND Ons ER = 
m | Pes) = DWH1(IX) ©1037 ( IPOW +I-1¢JJ¢ ITK-2) 
218 CONTINUE 
IX = HOD (EK 43) 1 
¥(30) = ane ) 8108S (IPIW+3 +IK-2) 
G =e Sos 2.2 2S * ARGE LOOF THAD OUTPUTS 
ast . PO. ! NL= NO 
iP(IY0 .B5. 2) NL # NC 
DO 400 L2=1,NL 
DO 250 £=1,30 
250 PSD(I) = 0.90 
Cc eee = = = OOS THEY FROCESS NOISE 
DO 300 I=1,8G 
DN1 = ON OR MS Q(T I) 
IP (Tyo . FO. 1. ANd. {Fr .£2. 1) WPITE(6, 8020) 
8020 PO HAT(/¢ TOUNGEER FUNenICY | FeOM) e20clSS NOiS= 
1,* BEASUPENENT ',12) 
LP(TYO 220; 2 -AMD. IPT .52~. 1) WRITE (6, 8030) 
8030 FGRMAT(/" TPAKSFER PUNCTION PHOS PROCESS NOTSE 
1® CONTAOL ',I2) 


gZ 


1) 





fe qmtU. 2008) GNLL RESID (I,L,N2,3S° +5 nen 
1 BSeBeRCofERL aesro (r.t.42.02" ss ey wamucan ve, 
Bi AS a ; peice Te 
1 pss Ba Ce bp T) : » NG,S0,NL,HU,HR,WI, 
Dom 26:0 *h=1-20 
7Z = DCMPLX(0. DO, 0.00) 
OM = w(K) 
DO 260 I[r=1,N2 
IF (WI (II) f 260,254,256 
254 ZD = DC* K ("aR (IT) ON-WI(IT) ) 
ZZ = RES (TI) /2D + 2% 
GO TO 26 
256 RE = WR{II 
eT ee, 
= Bias : + AI — 7M FAD 5 ~s 
ZN = DCMOLX( RES (IIL ¢1)FAI-RES Crs. eho DOF RE*GM 
72 <2 7 4 ahve | Cs ) =REORES (II) SOM) 
380 BSD # (PSD (K ON I*(2ZZ"DCC 
(K) = ) + (ZZ8DCON> 
300 CONTINUE 2Z)) 
(= eee se e222? GSUBY 
DO 320 I=1,N2 
DO 320 J=1,NO 
ST = 0.D0 
315 St ere Cae K) ©FBGE (K,J X(I,2 
bs = $ + eK) G oJ) + etSel) rw 
esi ev (Ted) = St N2,NO 9 GV,4,* (9 (1X, 1 ae ae 
. ‘ es ™~ = 
BqNo oo Asasiccerue DEsua’ AB Eas Py S 6) )°) 
Cc Diztiiz2t2 LOOP THRU EAS YOISE 
DO 390 I=1,NO 
asyee ; eieeer (een) R 
YU. EO. wAND, WRIT 
8040 PORRAT(7* TRANSFER FUNCTION” PPA MEAS dt Peet a es 
TTP(IYO £0202 20ND. IP WRI aes 
( £0. SAND. 
8050 FORMAT(/* TRANSFER FUNC ri6N “FROM sFis {Se Meee eS 
' ieee alee REGEDIT, LaN2 ICS ye 
EAE Ns . J Pe ee a ' 
BCG ERT Cay RESID (I, LyN2, JCP, 89, Sak aa 
4 et = Co td Ne Oe Ae ' 
1 a Ce, 1PT\ a. ei, V,NL,HU,WR,WI,RES, 
DO 380’ K=1, 30 
ZZ = DC“PLX (0. DO, 0.D0) 
OM = w(K) 
DO 360 IIl=1,N2 
LF (HI (TI) ) 360, 354 
354 ZD = NSCMP x fowk if tee WI (IID) 
Z2 = 22 ¢ RPS(LI)7/Zb 
GO TO 360 
356 PE=WR (TT) 
AI = Wi ( I) 
N= DEMPL fees (trea) #at- RES (rt geht, 
2N = MPLX(RE + 
Os eet Doe | (It) PE: RES (II) %0M) 
360 CONTINUE 
IF {IYO MEGG 2 2OR. I «NE. L) GO TO 374 
SD(K) = pS D(K) + DN 
378 P80 {k = PSD{K) * DNI* (ZZ*DCONIG (z2)) 
380 CONTINUF 
390 CONTINUE 
Tent) 5G. 1) sRITE 669039) L 
9000 rEiTtg £95 of) gthEae US Taye” FOR 
POZ“AT TPute . CED 
aN pASOSGALISED esDh is)” oa : BY ALL NOLS E- (PAD reboe 
FORPAT(/" PSD OF CONTEOL',I 3, POHCEn sae - 
1 ‘NOR FALIZED PSD /) HY ALL NOISE- (RAN ar eae a 
wRETE (6 9020) (W(T} ,PED(I), I=1 39) 
9020 POSSA (ax, (pets. ?, 2611.6 9) * 45 
400 CCOSTINUE 
RETUPRK 
1000 COYTINUE 


os 





ANAND 


9900 


CALL SREXILT (N2, PA, TERR) 
RETURN 


En ee ee ee 


SUBROOTINE EREXIT(N,A,IERR) 


EREXIT RETURNS THE NUMEER OF THE EIGENVALOE RHERE HQR2 
FAILS, THEN STOPS THE PROGRAM. 
INTEGER IERR 
DOUBLE PRECISION A 
DIN ENSION aa eee 
ae eee 4 TERR 
POR MA FATEURE, IN©HOR2 ON EIGENVALUE NO. *,23) 
CALL RAPENT(N,N,N,9,A,4, "(9 (1X, 1PD13. 5) ) *) 
a 
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cecceccececececececece cece ee cee cee CC Ce COC OST ec ceeccecececccce 


eccccececececececececece cee ceecee COCO CC Cec foe ececcececcccce 


C 
e SENSITIVITY COVARIANCE PROGEA Ss e 
c E 
e THIS PROGRAM IS USED TO SOLVE THE FEs59s ceyer 
C TOUATIONS WHEN THERE IS AN INCORRECT Peer es cart iron : 
C OF DYNAMICS IN THE DESIGN DF THE KALsan FILTER THE. OC 
C EQUATIONS BECOME < : 
¢ 
C PDOT= (F*-R89) Ft P(P®-KOH) T+D PVFVTDP IOS O35 T+KE RKO 
Cc VDOTHFV¢V(FS-K2H) T+UDFT-GOST eS rhea he = 
C ODIT=FU+UFT +GOGT : 
c THE PRINCIPAL PROGRAM INPUTS ARE TSE ForrowInG co- «& 
C LLECTION OF SYSTFS AND FILTER MATAicCSS x 
C PO THE INITIAL COVARIANCE MATRIX (XIN : 
C F THE TRUTH MODEL DYNAMICS MATROX ¢ yy C 
C Fo THE FILTER MODEL DYNAMICS MATSIS In xy z 
G H THE TRUTH MODEL MEASURENENT MATATX (LYN) WHERE 
S L IS THE MEASUREMENT VECTOR DIM=NS}on?’ Cc 
e GQGT THE INPUT NOISE COVABIANCE marhrx (wx C 
e R THE MEASUREMENT NOISE COVARIANCE? | ATRIX (LEL) C 
G Ke FILTER SAIN (NXL) 2 
c 
2 E 
C 
c THIS PROGRAMS HAS BEEN DEVELOPED USINS vue 
C AVAILABLE IN THE COMPOTER CENTER OF THE GaveTe ~TORARY 
C POSTGRADUATE SCHOOL 
e 
Mme set, 
Cc 1» My 4 e Ss e e wv 
@AKRRT (709) 2Oe (7 Merri eiy yf se 197 
SRSSKH IT, 7) GRU u Bs ae eI, seen, 
COMMON/KTAZN, NS, NPD 
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MES N lev O07. 7 
nV AR (105) oat 1n§) taht, re fu ehi2dy 
DIMENSION TH ae " TM 8747 P37 7) 
eperaAbeNce OUD GUAR CG LUD SEAR (hay coca, 
= V(2 r 
’ ODRV 7ah}° ( 9) tf (zd 1) ¢ 
C 
C N=ORDER OP THE SYSTEM MODEL 
é 
C NPSNUMBER OF POINTS 
% NPD=CONTPOL OF INITIAL DIAGNOSTIC DUrpUT 
G 
c DT3TISE INTERVAL 
EXTFRNAL PUN 
CALL UGETLO(3,5,6) 
C 
é THE FOLLOWING SECTION READS THE SPECIPYeED 7: 
C MATRICES,P,P*,GQGT,;K"H AND R eee HD 
¢ 
; READ (5,98) N,NP,NPD, DT 
98 PORSAT LATS 61075) ° 
WRITE (0, 97) % NP, 4YPD,DT 
97 FOS RAT WeotS, 1%,G )a. 10) 
NSeNe (Ne 1) 72 
Sizygen "261 
NV=2*NSe# HES 2 
<9 PC2 SAT (@F10.5) 
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TEND=FINAL TIME 
TEN D=K"®DT 


DVERK SUBROUTINE PINDS THE SOLUTION 2F THE SYSTEM OF 
DIFFERENTIAL EQUATIONS 

CALL DVERK(NV,PUK,T,VAR, TEND, TOL, IND, C, 105, WK, IER) 
IP(IND.LF. -OR. TER. NE Of STOP ; 

CALL VCVTSP(VAR(N1),N,PPULL,7) 

CALCULATE AND PRINT THE RMS ESTIMATE =RRORS 
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SUB FOUTINE FUN(NV,T,VAR, DRY) 
FCN SUBPOUTINE ITS USED FOR EVALUATING FUNCTIONS (INPUT) 
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C C 
C SENSITIVITY COVARIANCE PROGRAS c 
e THIS PROGRAM IS USED TO SOLVE [THE ERROE SENSITIVITY C 
E EQUATIONS WHEN THERE IS AN INCORRECT IMPLZMENTAITION C 
€ OF DYNAMICS IN THE LESIGN JF THE KALMAN PILTER.THE C 
C EQUATIONS BECOME C 
% PDO T= (FO -KSH) Pe (P*- KH) TeDEVEVTDPTSSQGT + KF RKAT E 
C VDOT=EVeV (FS KA H) T+UDPT-GQST C 
E UDI T= FU+OPT +GQGT Cc 
C e 
C THE PRINCIPAL PROGRAM INPUTS ARE THE POLLOWING CO- C 
C LLECTION OF SYSTEM AND FILTES MATRIC2S C 
E 
= PQ THE INITIAL COVARIANCE MATRIX (NXN) Cc 
Cc > THE TRUTH MODEL DYNANICS MATRIX (NXN) é 
é Fe THE PILTER NODSL DYNAMICS MATRIX (NXN) C 
C H THE TRUTH MODEL MEASUREMENT MATRIX (LXN) WHERE  C 
C L IS THE MEASUREMENT VECTOR DIM2NSION C 
C GQGT THE INPUT NOISE COVARIANCE MATRIX (NX) Cc 
Cc R THE MEASUREMENT NOISE COVARIANCE MATRIX (LXL) C 
C K® FILTER GAIN (NXL) e 
C 
gee cecece cececceeee cece cece cceess cceececcec sc ceccececececce 
é 
C THLS PROGRAS HAS BEEN DEVELOPED USING THE INSL LIBRARY 
Cc AVAILABLE IN THE COMPUTER CENTER OP THE NAVAL 
C POSTGRADUATE SCHOOL 
We 
a 
CGNHON (Bsa) SP [3.8] .g0gr 8) , AK (8¢3),8 (3,3 
SHON ; GOQGT (8) ,AK a 
AAKRKT g. 8) | DE ree ord)  FSSRAD (3.Of  SFe (O28), 
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DIMENSION PFOLL (8,8), PS2R(8) 
eb inca) ce435 44004961 ,19 9-0 
VAP (136) 9291198) c tak se (456, 9) Ley 3d)” : 
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; ODRV (101 
C N=ORDER OP THE SYSTE™ MODEL 
C 
C NP=NUMBEP OF POINTS 
C 
C NPD=CONTROL OF INITIAL DIAS NOSTIC OUTPUT 
C 
e DT=TIME INTERVAL 
C 
EXTERNAL PUN 
CALL UGETIO (3,5,6) 
C 
c 
é THE FOLLOWING SECTION RZADS THE SPECIFIED INPUT 
E MATRICES,F,F%,GQGT,K°R AND R 
Cc 
C 
READ (S, 98) NeNP NPD, DT 
98 FORRAT (31 £10. 5) 
WRITE ( OTS N KP,WPD,DT 
97 PORRAS | WOeat Sets G 185.10) 
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NIsNS¢kor le] 
NV2 26 8S¢ N75 2 
: 99 FORMAT(@F10.5) 
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TEND=FINAL TIME 
TEN D=KSDT 


DVEEK SUSROUTINE PINDS THE SOLUTION OF THE SYSTEX OP 
DIFFERENTIAL EQUATIONS 

CALL DVERK(NV,FUN,T,VAR, TEND, TOL, IND, C, 136,WK, ZEB) 
IF(IND.LE.O.OR.IER. NE.O} STOP 

CALL VCVTSF (VAR (N1) ,N,PPULL, 8) 

CALCULATE AND PRINT THE RMS ESTIMATE =ERPORS 
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SUBROUTINE FUN(NV,T,VAR, DRY) 
FCN SOBROUTINE IS USED FOR EVALUATINS PONCTIONS (INPUT) 


a] 


za 
im 
“YUN *& 8 I 


Ow Om » HH 


oor 
@ 


-Z) 
GOGT (8) ,AK (8,3) ,R (3,3), 
Bayne SKH IND NaNO 


Ow) oe oe ewes 
MN, OD OD 
&*3I® 

ose 
90D 
je 
pee 


fon 


ZawZOwHOr- 
oO 


Chew (1) EM EE 


en UD (36 VD(6,8), 
(5 4c (156, 4) P6030) 


= 


ee) 
i | 


Pipes aie terie 
F {RT.GE.5)_GO TO 15 
k 


Lobe 639) T 
OR MAT(*OT=',D25. 15) 


101 





aa) 
bad = 
= om fry 
* aa] » 
= »- m= 
= -_ a. » a 
- Mee fae. = m os 
Cm aa) ta N Zz —~ » [o @) - bo ol — - 
wy bed PY sey . ~ ow . Zz mm z - = 
> * ef iC a & » C4 » > CS = ~ 
i % a Esti » aA —- & ® L, ® : 
-~ 7 - 
i Ze eo & Bs oa: > s 4 . 
ha st) ad “ea » a | f+ J, ee a rs ® y, i > 
~ & em MY TF os ~ be fi & Roe = e rs = 
co ® =M%O «ee ~ © e Ohm m4) Qu — MO CO ® 
e ™% oft? oJe x » »* @ w+ OL ~~ f a, ~ > 
nat Ne Gab ie fia fe fa J Qa - Sw ad > Be ~m afew x Y: e ne 
fa FI > abe TF) ome » —“ ->COe OHOH O.0@ « Mm € om e 
Em OH Meer NH ~ eM * 8 & ran x e Fr ® 
F4 e sy fs et. 8 OY Fer.NCMs? Frm «© <_{ FF aa) 
© af") w LO 1 De = ~O. TOs O, « + Qie + ese Y e 
IO & —~, ef GM WM eB e EMF I Ze Pe oa: ae wt rs 
CO 8 we r) HE wim, + Bet Bene> FE4Q )?> & > 
OY Pak Mima Cw wma ren 5 ArkiLwoO—DnA, 41 Fo MeO © a) 
» Ooh eD we SMO ae §1D - > em a 1 — > teens OCR © 0) © 
Zz Fie Fae Nene ww Zhe we bw e Me Ze De NOe Mew we — 
om MeFi" a OKwowa--a’ et ee a=” 9%” 9 Qu. Od 03 —_ > 
Mam exon EOUE am wx NAHE Ces p a A Db} er 6.> (4. 
ee eh, ef, BO siete alo eh Out, wl Th ofs, Ee al, e:(5b. whe, «= 
af Zz we Ze ++] CBee UE p OE ee Bene 2 Be Eee | $m. 7% ¢+¢ xO “ 
Dr HD AWD emir OkhDp +ODMTmDMND2ZD Pee MHDs=D 
ee tm fy, eee *% EF A alt} WM ty e es ee » 4 
women we] Mee ee) o&) Nai AZ, wl>,) 4 fh, J 1 4 IH) ) — 
e & JZ] we pd > fa ed) FF et Yd] SJ “A ee — | oh | «< 
ene wt eat ae ol OF Aetet het fh, ot —~- et 7 a FS at ~ WU 
DP OWhUSDU QOOtOURUr Us) ~~ W~—UaUAU ove aw oa UF ap) 
seo“ = Lz - = —H oY so | Plea OH Sas VY am , nD —e 
SS) conf ¢ come Fa) cy swf tw fils te Oy alts pone cS com Sma KZ HE nom =_- HH Bm HH Bm HN 
FEAMNIWN  BSBUUmeMNLNIN SEDTOANnNANNMNaN BmAitmuwn eH WN mN . -_ eso ™“ e “se 
Vif WIV ord « © Qe’ op] ond op] 6 me efit 6D eel op] 6 eer em MT ol ee Ti oe | - A FfFeaT OQ = Ce 
KETBEDHDH KK AHP HD DA DH g—— NNR Re DEI DH KKM AQT ff KOR e Se " > wh => HH Qurd 
MMM DYAEA Wl © MALWARE Wee PHF YE th FY Be ew HOD | he oe 
PMD 6 6 RR om ote oh 8 RID) pe oe OHS oe Ib oS ss ~ ~ of Z 
te €Et “ & BF & & e 4b EF f+ “ —} Oo “Wt & mm NNR) Men 
Adder te Neremae yicAeA  M ORAM HTM NA K ODOT HX KAMMxA Ae I ee eS 
od) ] = Qu fa ed =] >] ] = ee at, No oi. .j1 ~~” oO =“ od fe od bh od > eA 
CleOstlaellye OOMR Ath etinet hy OOOOWLVUwetath. OOK OOKLA e Hh Om OOM Owrhwos 
WYWUUUHUFH AQARMUHUHUFRUH AAFP RPH HUANUHW AROHRUWHW DORR OUR A AAG AMAA M-4NWAS) 
Va) wrW wr Cc NO = ™N aa] 
- = - - - 
U U U YU U U U 


102 





LIST OF REFERENCES 


Agard, L. S. 95, Strapdown Inertial Navigation Systems, NATO Publi- 


cation, France, 1978. 


Maybeck, P. S., Stochastic Models, Estimation and Control, Vol. 1, 


Academic Press, 1979. 


Bryson, A. E., Kalman Filter Divergence and Aircraft Motion Esti- 
mators, Vol. 1, AIAA Journal, January 1978. 


Matallana, J. A., Sensitivity of The S.K.F. to Stability Derivatives 
ENSee 


Variations in An I Master's Thesis, Naval Postgraduate School, 
Monterey, 1980. 


Gelb, A. and others, Applied Optimal Estimation, MIT Press, 1974. 


Franklin, G. F. and Powell, J. D., Digital Control of Dynamic Systems, 
Addison-Wesley, 1980. 


Anderson, B. D. 0., and Moore, J. B., Linear System Optimization with 


Prescribed Degree of Stability, Proc. IEE, Vol. 116, No. 12, December 
1969. 


Collins, D. J., Missiles, Navigation and Avionic Systems, Class Notes, 
Naval. Postgraduate School, October 1981. 


Walker, R., OPTSYS 4 at SCIP Computer Program, Stanford University, 
Aero/Astro Department, December 1979. 


. Potter, G. G., An Aid to Using OPTSYS at NPS, Class Term Paper, Naval 
Postgraduate School, March 1982. 


. Stansell, T. A., Jr., The Many Faces of Transit, Vol. 25, No. 1, 
Journal of the Institude of Navigation, Spring 1978. 


103 





INITIAL DISTRIBUTION LIST 


No. Copies 


Defense Technical Information Center Z 
Cameron Station 
Alexandria, Virginia 22314 


Library, Code 0142 2 
Naval Postgraduate Schoo] 
Monterey, California 93940 


Department Chairman, Code 62 ] 
Department of Electrical Engineering 

Naval Postgraduate Schoo] 

Monterey, California 93940 


Professor D. J. Collins, Code 6/Co 2 
Department of Aeronautics 

Naval Postgraduate Schoo] 

Monterey, California 93940 


Professor H. A. Titus, Code 62Ts ] 
Department of Electrical Engineering 

Naval Postgraduate School 

Monterey, California 93940 


Lcedr. Gary G. Potter 2 
Charles Stark Draper Laboratory 

555 Technology Square 

Cambridge, Massachusetts 02139 


104 























